Copter: WP Pause support

ArduCopter/GCS_Mavlink.cpp

@@ -1003,15 +1003,16 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_pause_continue(const mavlink_comma
 
     // requested pause from GCS
     if ((int8_t) packet.param1 == 0) {
-        copter.mode_auto.mission.stop();
-        copter.mode_auto.loiter_start();
+        // copter.mode_auto.mission.stop();
+        copter.mode_auto.pause_mission();
         gcs().send_text(MAV_SEVERITY_INFO, "Paused mission");
         return MAV_RESULT_ACCEPTED;
     }
 
     // requested resume from GCS
     if ((int8_t) packet.param1 == 1) {
-        copter.mode_auto.mission.resume();
+        // copter.mode_auto.mission.resume();
+        copter.mode_auto.continue_mission();
         gcs().send_text(MAV_SEVERITY_INFO, "Resumed mission");
         return MAV_RESULT_ACCEPTED;
     }

ArduCopter/mode.h

@@ -427,6 +427,9 @@ public:
     void circle_movetoedge_start(const Location &circle_center, float radius_m);
     void circle_start();
     void nav_guided_start();
+    
+    void pause_mission();
+    void continue_mission();
 
     bool is_landing() const override;
 

ArduCopter/mode_auto.cpp

@@ -673,6 +673,18 @@ void ModeAuto::exit_mission()
     }
 }
 
+// pause_mission - Prevent aircraft from progressing along the track
+void ModeAuto::pause_mission()
+{
+        wp_nav->set_pause();
+}
+
+// continue_mission - Allow aircraft to progress along the track
+void ModeAuto::continue_mission()
+{
+        wp_nav->set_continue();
+}
+
 // do_guided - start guided mode
 bool ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd)
 {