Plane: bug fix to set_current_cmd

When set_current_cmd mavlink message is received, resume mission only if
the mission has completed.
Stop mission when vehicle leaves AUTO mode.

ArduPlane/GCS_Mavlink.pde

@@ -1606,13 +1606,15 @@ mission_item_send_failed:
         if (mavlink_check_target(packet.target_system,packet.target_component)) break;
 
         // set current command
-        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);
+        if (mission.set_current_cmd(packet.seq)) {
+            // restart/resume mission if we are in auto but the mission is not running (i.e. complete)
+            if (control_mode == AUTO && mission.state() != AP_Mission::MISSION_RUNNING) {
+                mission.resume();
+            }
         }
+
+        // update GCS with out mission command
+        mavlink_msg_mission_current_send(chan, mission.get_current_nav_cmd().index);
         break;
     }
 

ArduPlane/system.pde

@@ -308,6 +308,10 @@ static void set_mode(enum FlightMode mode)
     if(g.auto_trim > 0 && control_mode == MANUAL)
         trim_control_surfaces();
 
+    // perform any cleanup required for prev flight mode
+    exit_mode(control_mode);
+
+    // set mode
     control_mode = mode;
 
     switch(control_mode)
@@ -388,6 +392,17 @@ static void set_mode(enum FlightMode mode)
     yawController.reset_I();    
 }
 
+// exit_mode - perform any cleanup required when leaving a flight mode
+static void exit_mode(enum FlightMode mode)
+{
+    // stop mission when we leave auto
+    if (mode == AUTO) {
+        if (mission.state() == AP_Mission::MISSION_RUNNING) {
+            mission.stop();
+        }
+    }
+}
+
 static void check_long_failsafe()
 {
     uint32_t tnow = millis();