Copter: allow to resume in ZigZag Auto

ArduCopter/RC_Channel.cpp

@@ -485,6 +485,8 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw
         case AUX_FUNC::ZIGZAG_SaveWP:
 #if MODE_ZIGZAG_ENABLED == ENABLED
             if (copter.flightmode == &copter.mode_zigzag) {
+                // initialize zigzag auto
+                copter.mode_zigzag.init_auto();
                 switch (ch_flag) {
                     case LOW:
                         copter.mode_zigzag.save_or_move_to_destination(ModeZigZag::Destination::A);
@@ -570,6 +572,7 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw
                     copter.mode_zigzag.run_auto();
                     break;
                 default:
+                    copter.mode_zigzag.suspend_auto();
                     break;
                 }
             }

ArduCopter/mode.h

@@ -1402,7 +1402,11 @@ public:
     bool init(bool ignore_checks) override;
     void exit();
     void run() override;
+
+    // auto control methods.  copter flies grid pattern
     void run_auto();
+    void suspend_auto();
+    void init_auto();
 
     bool requires_GPS() const override { return true; }
     bool has_manual_throttle() const override { return false; }
@@ -1432,6 +1436,8 @@ private:
 
     Vector2f dest_A;    // in NEU frame in cm relative to ekf origin
     Vector2f dest_B;    // in NEU frame in cm relative to ekf origin
+    Vector3f current_dest; // current target destination (use for resume after suspending)
+    bool current_terr_alt;
 
     enum ZigZagState {
         STORING_POINTS, // storing points A and B, pilot has manual control
@@ -1440,7 +1446,7 @@ private:
     } stage;
 
     enum AutoState {
-        MANUAL,         // after initializing, before switching to ZigZag Auto
+        MANUAL,         // not in ZigZag Auto
         AB_MOVING,      // moving from A to B or from B to A
         SIDEWAYS,       // moving to sideways
     } auto_stage;
@@ -1450,6 +1456,7 @@ private:
     bool is_auto;                   // enable zigzag auto feature which is automate both AB and sideways
     uint16_t line_count = 0;        // current line number
     int16_t line_num = 0;           // target line number
+    bool is_suspended;              // true if zigzag auto is suspended
 };
 
 #if MODE_AUTOROTATE_ENABLED == ENABLED

ArduCopter/mode_zigzag.cpp

@@ -7,6 +7,7 @@
 */
 
 #define ZIGZAG_WP_RADIUS_CM 300
+#define ZIGZAG_LINE_INFINITY -1
 
 // initialise zigzag controller
 bool ModeZigZag::init(bool ignore_checks)
@@ -37,9 +38,9 @@ bool ModeZigZag::init(bool ignore_checks)
     stage = STORING_POINTS;
     dest_A.zero();
     dest_B.zero();
-    auto_stage = AutoState::MANUAL;
 
-    line_count = 0;
+    // initialize zigzag auto
+    init_auto();
 
     return true;
 }
@@ -72,7 +73,7 @@ void ModeZigZag::run()
             // if vehicle has reached destination switch to manual control or moving to A or B
             AP_Notify::events.waypoint_complete = 1;
             if (is_auto) {
-                if (line_num == -1 || line_count < line_num) {
+                if (line_num == ZIGZAG_LINE_INFINITY || line_count < line_num) {
                     if (auto_stage == AutoState::SIDEWAYS) {
                         save_or_move_to_destination((ab_dest_stored == Destination::A) ? Destination::B : Destination::A);
                     } else {
@@ -81,8 +82,7 @@ void ModeZigZag::run()
                         move_to_side();
                     }
                 } else {
-                    is_auto = false;
-                    line_count = 0;
+                    init_auto();
                     return_to_manual_control(true);
                 }
             } else {
@@ -147,7 +147,7 @@ void ModeZigZag::save_or_move_to_destination(Destination ab_dest)
                     // spray on while moving to A or B
                     spray(true);
                     reach_wp_time_ms = 0;
-                    if (is_auto == false || line_num == -1) {
+                    if (is_auto == false || line_num == ZIGZAG_LINE_INFINITY) {
                         gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: moving to %s", (ab_dest == Destination::A) ? "A" : "B");
                     } else {
                         line_count++;
@@ -169,6 +169,8 @@ void ModeZigZag::move_to_side()
             if (wp_nav->set_wp_destination(next_dest, terr_alt)) {
                 stage = AUTO;
                 auto_stage = AutoState::SIDEWAYS;
+                current_dest = next_dest;
+                current_terr_alt = terr_alt;
                 reach_wp_time_ms = 0;
                 char const *dir[] = {"forward", "right", "backward", "left"};
                 gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: moving to %s", dir[(uint8_t)zigzag_direction]);
@@ -193,7 +195,6 @@ void ModeZigZag::return_to_manual_control(bool maintain_target)
         } else {
             loiter_nav->init_target();
         }
-        auto_stage = AutoState::MANUAL;
         is_auto = false;
         gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: manual control");
     }
@@ -469,8 +470,48 @@ bool ModeZigZag::calculate_side_dest(Vector3f& next_dest, bool& terrain_alt) con
 // run zigzag auto feature which is automate both AB and sideways
 void ModeZigZag::run_auto()
 {
+    // make sure both A and B point are registered and not when moving to A or B
+    if (stage != MANUAL_REGAIN) {
+        return;
+    }
+
     is_auto = true;
-    move_to_side();
+    // resume if zigzag auto is suspended
+    if (is_suspended && line_count <= line_num) {
+        // resume the stage when it was suspended
+        if (auto_stage == AutoState::AB_MOVING) {
+            line_count--;
+            save_or_move_to_destination(ab_dest_stored);
+        } else if (auto_stage == AutoState::SIDEWAYS) {
+            wp_nav->wp_and_spline_init();
+            if (wp_nav->set_wp_destination(current_dest, current_terr_alt)) {
+                stage = AUTO;
+                reach_wp_time_ms = 0;
+                char const *dir[] = {"forward", "right", "backward", "left"};
+                gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: moving to %s", dir[(uint8_t)zigzag_direction]);
+            }
+        }
+    } else {
+        move_to_side();
+    }
+}
+
+// suspend zigzag auto
+void ModeZigZag::suspend_auto()
+{
+    if (auto_stage != AutoState::MANUAL) {
+        is_suspended = true;
+        return_to_manual_control(true);
+    }
+}
+
+// initialize zigzag auto
+void ModeZigZag::init_auto()
+{
+    is_auto = false;
+    auto_stage = AutoState::MANUAL;
+    line_count = 0;
+    is_suspended = false;
 }
 
 // spray on / off