DroneGroup: RTL_ALT_M and WP_ACC

Author: EosBandi

Swarm/WaypointLeader/DroneGroup.cs

@@ -130,9 +130,9 @@ public void UpdatePositions()
                                 return;
                             }
                         }
-                    }
-
-                    // check how close they are based on a 1 second projection
+                    }
+
+                    // check how close they are based on a 1 second projection
                     if (drone1.ProjectedLocation.GetDistance(drone2.ProjectedLocation) < Seperation / 2)
                     {
                         // check if they are heading the same direction
@@ -192,9 +192,9 @@ public void UpdatePositions()
                 if (locs.Count == 0)
                     continue;
                 drone.PathIndex = path.IndexOf(locs[0]);
-            }
-
-
+            }
+
+
             switch (CurrentMode)
             {
                 case Mode.idle:
@@ -213,20 +213,22 @@ public void UpdatePositions()
                         {
                             try
                             {
-                                // get param
-                                MAV.parent.GetParam(MAV.sysid, MAV.compid, "RTL_ALT");
-                                // set param
-                                MAV.parent.setParam(MAV.sysid, MAV.compid, "RTL_ALT", 0); // cms - rtl at current alt
+                                // rtl at current alt
+                                if (MAV.param.ContainsKey("RTL_ALT"))
+                                    MAV.parent.setParam(MAV.sysid, MAV.compid, "RTL_ALT", 0);
+                                else if (MAV.param.ContainsKey("RTL_ALT_M"))
+                                    MAV.parent.setParam(MAV.sysid, MAV.compid, "RTL_ALT_M", 0);
                             }
                             catch
                             {
                             }
                             try
                             {
-                                // get param
-                                MAV.parent.GetParam(MAV.sysid, MAV.compid, "WPNAV_ACCEL");
-                                // set param to default 100cm/s
-                                MAV.parent.setParam(MAV.sysid, MAV.compid, "WPNAV_ACCEL", 100);
+                                // set accel to default
+                                if (MAV.param.ContainsKey("WPNAV_ACCEL"))
+                                    MAV.parent.setParam(MAV.sysid, MAV.compid, "WPNAV_ACCEL", 100); // 100 cm/s/s
+                                else if (MAV.param.ContainsKey("WP_ACC"))
+                                    MAV.parent.setParam(MAV.sysid, MAV.compid, "WP_ACC", 1); // 1 m/s/s
                             }
                             catch
                             {
@@ -280,7 +282,7 @@ public void UpdatePositions()
                                     0, takeoffalt))
                                 {
                                     drone.takeoffdone = true;
-                                }
+                                }
                         }
                         catch (Exception ex)
                         {
@@ -390,7 +392,10 @@ public void UpdatePositions()
                                 continue;
                             var MAV = drone.MavState;
                             // update to faster speed
-                            MAV.parent.setParam(MAV.sysid, MAV.compid, "WPNAV_ACCEL", (float)WPNAV_ACCEL * 100.0f);
+                            if (MAV.param.ContainsKey("WPNAV_ACCEL"))
+                                MAV.parent.setParam(MAV.sysid, MAV.compid, "WPNAV_ACCEL", (float)WPNAV_ACCEL * 100.0f);
+                            else if (MAV.param.ContainsKey("WP_ACC"))
+                                MAV.parent.setParam(MAV.sysid, MAV.compid, "WP_ACC", (float)WPNAV_ACCEL);
 
                             MAV.parent.setMode(MAV.sysid, MAV.compid, "GUIDED");
                         }
@@ -482,9 +487,11 @@ public void UpdatePositions()
                         try
                         {
                             var MAV = drone.MavState;
-                            // set param to default 100cm/s
-                            if (MAV.param["WPNAV_ACCEL"].Value != 100)
-                                MAV.parent.setParam(MAV.sysid, MAV.compid, "WPNAV_ACCEL", 100);
+                            // set accel to default
+                            if (MAV.param.ContainsKey("WPNAV_ACCEL") && MAV.param["WPNAV_ACCEL"].Value != 100)
+                                MAV.parent.setParam(MAV.sysid, MAV.compid, "WPNAV_ACCEL", 100); // 100 cm/s/s
+                            else if (MAV.param.ContainsKey("WP_ACC") && MAV.param["WP_ACC"].Value != 1)
+                                MAV.parent.setParam(MAV.sysid, MAV.compid, "WP_ACC", 1); // 1 m/s/s
                         }
                         catch
                         {
@@ -521,9 +528,11 @@ public void UpdatePositions()
                         try
                         {
                             var MAV = drone.MavState;
-                            // set param to default 100cm/s
-                            if (MAV.param["WPNAV_ACCEL"].Value != 100)
-                                MAV.parent.setParam(MAV.sysid, MAV.compid, "WPNAV_ACCEL", 100);
+                            // set accel to default
+                            if (MAV.param.ContainsKey("WPNAV_ACCEL") && MAV.param["WPNAV_ACCEL"].Value != 100)
+                                MAV.parent.setParam(MAV.sysid, MAV.compid, "WPNAV_ACCEL", 100); // 100 cm/s/s
+                            else if (MAV.param.ContainsKey("WP_ACC") && MAV.param["WP_ACC"].Value != 1)
+                                MAV.parent.setParam(MAV.sysid, MAV.compid, "WP_ACC", 1); // 1 m/s/s
                         }
                         catch
                         {
@@ -640,7 +649,7 @@ double GetOffPathDistance(List<PointLatLngAlt> path, PointLatLngAlt Location)
 
         private List<PointLatLngAlt> GetLocationsV(List<PointLatLngAlt> path, PointLatLngAlt location, double lead,
             double seperation)
-        {
+        {
             List<PointLatLngAlt> result = new List<PointLatLngAlt>();
             var list = GetLocations(path, location, lead, seperation);