Plane: fixed relative altitudes and resuming mission

Pair-Programmed-With: Randy Mackay <rmackay9@yahoo.com>

ArduPlane/GCS_Mavlink.pde

@@ -1606,7 +1606,11 @@ mission_item_send_failed:
         if (mavlink_check_target(packet.target_system,packet.target_component)) break;
 
         // set current command
-        if (mission.set_current_cmd(packet.seq)) {
+        if (packet.seq == 0) {
+            mission.start();
+            mavlink_msg_mission_current_send(chan, mission.get_current_nav_cmd().index);
+        } else if (mission.set_current_cmd(packet.seq)) {
+            mission.resume();
             mavlink_msg_mission_current_send(chan, mission.get_current_nav_cmd().index);
         }
         break;
@@ -1628,12 +1632,7 @@ mission_item_send_failed:
             break;
         }
 
-        // new mission arriving, clear current mission
-        if (!mission.clear()) {
-            // return error if we were unable to clear the mission (possibly because we're currently flying the mission)
-            mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ERROR);
-            break;
-        }
+        mission.truncate(packet.count);
 
         waypoint_timelast_receive = millis();
         waypoint_timelast_request = 0;

ArduPlane/commands_logic.pde

@@ -160,6 +160,7 @@ static void exit_mission()
     gcs_send_text_fmt(PSTR("Returning to Home"));
     next_nav_command = rally_find_best_cmd(current_loc, home);
     next_nav_command.id = MAV_CMD_NAV_LOITER_UNLIM;
+    setup_glide_slope();
     start_command(next_nav_command);
 }
 

ArduPlane/commands_process.pde

@@ -11,7 +11,7 @@ static void update_commands(void)
         if(home_is_set == true && mission.num_commands() > 1) {
             if(mission.state() == AP_Mission::MISSION_RUNNING) {
                 mission.update();
-            }else if(mission.state() == AP_Mission::MISSION_COMPLETE) {
+            } else {
                 // next_nav_command should have been set to MAV_CMD_NAV_LOITER_UNLIM by exit_mission
                 verify_command(next_nav_command);
             }

ArduPlane/control_modes.pde

@@ -48,11 +48,9 @@ static void read_control_switch()
 
     if (g.reset_mission_chan != 0 &&
         hal.rcin->read(g.reset_mission_chan-1) > RESET_SWITCH_CHAN_PWM) {
-        if (mission.set_current_cmd(0)) {
-            // reset to first waypoint in mission
+        mission.start();
         prev_WP.content.location = current_loc;
     }
-    }
 
     switch_debouncer = false;
 

ArduPlane/rally.pde

@@ -66,7 +66,9 @@ static bool find_best_rally_point(const Location &myloc, const Location &homeloc
 static Location rally_location_to_location(const RallyLocation &r_loc, const Location &homeloc)
 {
     Location ret = {};
-    ret.flags.relative_alt = true;
+
+    // we return an absolute altitude, as we add homeloc.alt below
+    ret.flags.relative_alt = false;
 
     //Currently can't do true AGL on the APM.  Relative altitudes are
     //relative to HOME point's altitude.  Terrain on the board is inbound
@@ -92,6 +94,8 @@ static AP_Mission::Mission_Command rally_find_best_cmd(const Location &myloc, co
         ret.content.location = homeloc;
         // Altitude to hold over home
         ret.content.location.alt = read_alt_to_hold();
+        // read_alt_to_hold returns an absolute altitude
+        ret.content.location.flags.relative_alt = false;
     }
     return ret;
 }