Plane: added error checking to rally load save

sanity check the rally points

ArduPlane/GCS_Mavlink.pde

@@ -1816,18 +1816,25 @@ mission_failed:
         if (mavlink_check_target(packet.target_system, packet.target_component))
             break;
         
+        if (packet.idx >= g.rally_total || 
+            packet.idx >= MAX_RALLYPOINTS) {
+            send_text_P(SEVERITY_LOW,PSTR("bad rally point message ID"));
+            break;
+        }
+
         if (packet.count != g.rally_total) {
-            send_text_P(SEVERITY_LOW,PSTR("bad rally point message"));
-        } else {
-            RallyLocation rally_point;
-            rally_point.lat = packet.lat;
-            rally_point.lng = packet.lng;
-            rally_point.alt = packet.alt;
-            rally_point.break_alt = packet.break_alt;
-            rally_point.land_dir = packet.land_dir;
-            rally_point.flags = packet.flags;
-            set_rally_point_with_index(rally_point, packet.idx);
+            send_text_P(SEVERITY_LOW,PSTR("bad rally point message count"));
+            break;
         }
+
+        RallyLocation rally_point;
+        rally_point.lat = packet.lat;
+        rally_point.lng = packet.lng;
+        rally_point.alt = packet.alt;
+        rally_point.break_alt = packet.break_alt;
+        rally_point.land_dir = packet.land_dir;
+        rally_point.flags = packet.flags;
+        set_rally_point_with_index(packet.idx, rally_point);
         break;
     }
 
@@ -1838,12 +1845,19 @@ mission_failed:
         if (mavlink_check_target(packet.target_system, packet.target_component))
             break;
         if (packet.idx > g.rally_total) {
-            send_text_P(SEVERITY_LOW, PSTR("bad rally point"));   
-        } else {
-            RallyLocation rally_point = get_rally_point_with_index(packet.idx);
-            mavlink_msg_rally_point_send(chan, msg->sysid, msg->compid, packet.idx, g.rally_total, rally_point.lat, rally_point.lng, rally_point.alt, rally_point.break_alt, rally_point.land_dir, rally_point.flags);
+            send_text_P(SEVERITY_LOW, PSTR("bad rally point index"));   
+            break;
+        }
+        RallyLocation rally_point;
+        if (!get_rally_point_with_index(packet.idx, rally_point)) {
+            send_text_P(SEVERITY_LOW, PSTR("failed to set rally point"));   
+            break;
         }
 
+        mavlink_msg_rally_point_send(chan, msg->sysid, msg->compid, packet.idx, 
+                                     g.rally_total, rally_point.lat, rally_point.lng, 
+                                     rally_point.alt, rally_point.break_alt, rally_point.land_dir, 
+                                     rally_point.flags);
         break;
     }    
 

ArduPlane/Parameters.pde

@@ -231,7 +231,7 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Param: FENCE_TOTAL
     // @DisplayName: Fence Total
     // @Description: Number of geofence points currently loaded
-    // @User: Standard
+    // @User: Advanced
     GSCALAR(fence_total,            "FENCE_TOTAL",    0),
 
     // @Param: FENCE_CHANNEL
@@ -262,7 +262,7 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Param: RALLY_TOTAL
     // @DisplayName: Rally Total
     // @Description: Number of rally points currently loaded
-    // @User: Standard
+    // @User: Advanced
     GSCALAR(rally_total,            "RALLY_TOTAL",    0),
 
     // @Param: ARSPD_FBW_MIN

ArduPlane/commands_logic.pde

@@ -234,18 +234,16 @@ static void do_RTL(void)
     control_mode    = RTL;
     prev_WP = current_loc;
 
-    if ((unsigned) g.rally_total.get() < 1) {
-        next_WP = home;
-
-        // Altitude to hold over home
-        // Set by configuration tool
-        // -------------------------
-        next_WP.alt = read_alt_to_hold();
-
-    } else { //we have setup Rally points: use them instead of Home for RTL
-        RallyLocation ral_loc = find_best_rally_point();
-
-        next_WP = rally_location_to_location(ral_loc);
+    RallyLocation ral_loc;
+    if (find_best_rally_point(current_loc, home, ral_loc)) {
+        //we have setup Rally points: use them instead of Home for RTL
+        next_WP = rally_location_to_location(ral_loc, home);
+    } else {
+         next_WP = home;
+         // Altitude to hold over home
+         // Set by configuration tool
+         // -------------------------
+         next_WP.alt = read_alt_to_hold();
     }
 
     if (g.loiter_radius < 0) {

ArduPlane/rally.pde

@@ -5,88 +5,60 @@
  */
 
 //get a rally point
-static RallyLocation get_rally_point_with_index(unsigned i)
+static bool get_rally_point_with_index(unsigned i, RallyLocation &ret)
 {
-    uint16_t mem;
-    RallyLocation ret;
-
-    if (i > (unsigned) g.rally_total.get()) {
-        ret.lat = 0; ret.lng = 0; ret.alt = 0;
-        ret.land_dir = 0; ret.flags = 0;
-        return ret;
+    if (i >= (unsigned)g.rally_total) {
+        return false;
     }
-
-    //read rally point
-    mem = RALLY_START_BYTE + (i * RALLY_WP_SIZE);
-    ret.lat = hal.storage->read_dword(mem);
-    mem += sizeof(uint32_t);
-    ret.lng = hal.storage->read_dword(mem);
-    mem += sizeof(uint32_t);
-    ret.alt = hal.storage->read_word(mem);
-    mem += sizeof(int16_t);
-    ret.break_alt = hal.storage->read_word(mem);
-    mem += sizeof(int16_t);
-    ret.land_dir = hal.storage->read_word(mem);
-    mem += sizeof(uint16_t);
-    ret.flags = hal.storage->read_byte(mem);
-
-    return ret;    
+    hal.storage->read_block(&ret, RALLY_START_BYTE + (i * sizeof(RallyLocation)), sizeof(RallyLocation));
+    if (ret.lat == 0 && ret.lng == 0) {
+        // sanity check ...
+        return false;
+    }
+    return true; 
 }
 
 //save a rally point
-static void set_rally_point_with_index(RallyLocation &rallyLoc, unsigned i)
+static bool set_rally_point_with_index(unsigned i, const RallyLocation &rallyLoc)
 {
-    uint16_t mem;
-
-    if (i >= (unsigned) g.rally_total.get()) {
+    if (i >= (unsigned) g.rally_total) {
         //not allowed
-        return;
+        return false;
     }
 
     if (i >= MAX_RALLYPOINTS) {
         //also not allowed
-        return;
+        return false;
     }
-
-    mem = RALLY_START_BYTE + (i * RALLY_WP_SIZE);
-
-    hal.storage->write_dword(mem, rallyLoc.lat);
-    mem += sizeof(uint32_t);
-    hal.storage->write_dword(mem, rallyLoc.lng);
-    mem += sizeof(uint32_t);
-    hal.storage->write_word(mem, rallyLoc.alt);
-    mem += sizeof(int16_t);
-    hal.storage->write_word(mem, rallyLoc.break_alt);
-    mem += sizeof(int16_t);
-    hal.storage->write_word(mem, rallyLoc.land_dir);
-    mem += sizeof(uint32_t);
-    hal.storage->write_byte(mem, rallyLoc.flags);
+    hal.storage->write_block(RALLY_START_BYTE + (i * sizeof(RallyLocation)), &rallyLoc, sizeof(RallyLocation));
+    return true;
 }
 
-//'best' means 'closest to my current location' for now.
-RallyLocation find_best_rally_point() {
-    RallyLocation ret;
-    RallyLocation next_rally;
-    Location rally_loc;
-    float dis;
-    float min_dis = 999999999.9f;
+// 'best' means 'closest to Location loc' for now.
+static bool find_best_rally_point(const Location &myloc, const Location &homeloc, RallyLocation &ret) 
+{
+    float min_dis = -1;
 
-    for (unsigned i = 0; i < (unsigned) g.rally_total.get(); i++) {
-        next_rally = get_rally_point_with_index(i);
-        rally_loc = rally_location_to_location(next_rally);
-        dis = get_distance(current_loc, rally_loc);
+    for (unsigned i = 0; i < (unsigned) g.rally_total; i++) {
+        RallyLocation next_rally;
+        if (!get_rally_point_with_index(i, next_rally)) {
+            continue;
+        }
+        Location rally_loc = rally_location_to_location(next_rally, homeloc);
+        float dis = get_distance(myloc, rally_loc);
 
-        if (dis < min_dis) {
+        if (dis < min_dis || min_dis < 0) {
             min_dis = dis;
             ret = next_rally;
         }
     }
 
-    return ret;
+    return min_dis >= 0;
 }
 
-//translate a RallyLocation to a Location
-Location rally_location_to_location (const RallyLocation &r_loc) {
+// translate a RallyLocation to a Location
+static Location rally_location_to_location(const RallyLocation &r_loc, const Location &homeloc) 
+{
     Location ret;
 
     ret.id = MAV_CMD_NAV_LOITER_UNLIM;
@@ -95,7 +67,7 @@ Location rally_location_to_location (const RallyLocation &r_loc) {
     //Currently can't do true AGL on the APM.  Relative altitudes are
     //relative to HOME point's altitude.  Terrain on the board is inbound
     //for the PX4, though.  This line will need to be updated when that happens:
-    ret.alt = r_loc.alt + home.alt;
+    ret.alt = r_loc.alt + homeloc.alt;
 
     ret.lat = r_loc.lat;
     ret.lng = r_loc.lng;