Copter: revamp zigzag mode

more accurately stops on target
uses current z target when moving to edge
loses unnecessary auto_yaw.set_mode
ensure A and B are different

ArduCopter/RC_Channel.cpp

@@ -104,6 +104,7 @@ void RC_Channel_Copter::init_aux_function(const aux_func_t ch_option, const aux_
     case USER_FUNC2:
     case USER_FUNC3:
     case ZIGZAG:
+    case ZIGZAG_SaveWP:
         break;
     default:
         RC_Channel::init_aux_function(ch_option, ch_flag);
@@ -527,9 +528,25 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw
 #endif
 
         case ZIGZAG:
+#if MODE_ZIGZAG_ENABLED == ENABLED
+            do_aux_function_change_mode(control_mode_t::ZIGZAG, ch_flag);
+#endif
+            break;
+
+        case ZIGZAG_SaveWP:
 #if MODE_ZIGZAG_ENABLED == ENABLED
             if (copter.flightmode == &copter.mode_zigzag) {
-                copter.mode_zigzag.receive_signal_from_auxsw(ch_flag);
+                switch (ch_flag) {
+                    case LOW:
+                        copter.mode_zigzag.save_or_move_to_destination(0);
+                        break;
+                    case MIDDLE:
+                        copter.mode_zigzag.return_to_manual_control();
+                        break;
+                    case HIGH:
+                        copter.mode_zigzag.save_or_move_to_destination(1);
+                        break;
+                }
             }
 #endif
             break;

ArduCopter/mode.h

@@ -1270,7 +1270,11 @@ public:
     bool allows_arming(bool from_gcs) const override { return false; }
     bool is_autopilot() const override { return true; }
 
-    void receive_signal_from_auxsw(RC_Channel::aux_switch_pos_t aux_switch_position);
+    // save current position as A (dest_num = 0) or B (dest_num = 1).  If both A and B have been saved move to the one specified
+    void save_or_move_to_destination(uint8_t dest_num);
+
+    // return manual control to the pilot
+    void return_to_manual_control();
 
 protected:
 
@@ -1281,31 +1285,17 @@ private:
 
     void auto_control();
     void manual_control();
-    bool has_arr_at_dest();
-    bool calculate_next_dest(Vector3f& next_dest, RC_Channel::aux_switch_pos_t next_A_or_B) const;
-    bool set_destination(const Vector3f& destination, RC_Channel::aux_switch_pos_t aux_switch_position);
+    bool reached_destination();
+    bool calculate_next_dest(uint8_t position_num, Vector3f& next_dest) const;
 
-    struct {   
-        Vector2f A_pos; // in NEU frame in cm relative to home location
-        Vector2f B_pos; // in NEU frame in cm relative to home location
-        RC_Channel::aux_switch_pos_t switch_pos_A;  // switch position recorded as point A
-        RC_Channel::aux_switch_pos_t switch_pos_B;  // switch position recorded as point B
-    } zigzag_waypoint;
+    Vector2f dest_A;    // in NEU frame in cm relative to ekf origin
+    Vector2f dest_B;    // in NEU frame in cm relative to ekf origin
 
     enum zigzag_state {
-        REQUIRE_A,       // point A is not defined yet, pilot has manual control
-        REQUIRE_B,       // point B is not defined but A has been defined, pilot has manual control
-        AUTO,            // after A and B defined, pilot toggle the switch from one side to the other, vehicle flies autonomously
-        MANUAL_REGAIN    // pilot toggle the switch to middle position, has manual control
-    };
-
-    zigzag_state stage = REQUIRE_A;
-
-    struct {
-        uint32_t last_judge_pos_time;
-        Vector3f last_pos;
-        bool is_keeping_time;
-    } zigzag_judge_moving;
+        STORING_POINTS, // storing points A and B, pilot has manual control
+        AUTO,           // after A and B defined, pilot toggle the switch from one side to the other, vehicle flies autonomously
+        MANUAL_REGAIN   // pilot toggle the switch to middle position, has manual control
+    } stage;
 
-    bool zigzag_is_between_A_and_B;
+    uint32_t reach_wp_time_ms = 0;  // time since vehicle reached destination (or zero if not yet reached)
 };

ArduCopter/mode_zigzag.cpp

@@ -6,9 +6,9 @@
 * Init and run calls for zigzag flight mode
 */
 
-#define ZIGZAG_WP_RADIUS_SQUARED  9
+#define ZIGZAG_WP_RADIUS_CM 300
 
-// init - initialise zigzag controller
+// initialise zigzag controller
 bool Copter::ModeZigZag::init(bool ignore_checks)
 {
     if (!copter.position_ok() && !ignore_checks) {
@@ -16,6 +16,7 @@ bool Copter::ModeZigZag::init(bool ignore_checks)
     }
 
     // initialize's loiter position and velocity on xy-axes from current pos and velocity
+    loiter_nav->clear_pilot_desired_acceleration();
     loiter_nav->init_target();
 
     // initialise position_z and desired velocity_z
@@ -25,13 +26,14 @@ bool Copter::ModeZigZag::init(bool ignore_checks)
     }
 
     // initialise waypoint state
-    zigzag_is_between_A_and_B = false;
-    zigzag_judge_moving.is_keeping_time = false;
-    stage = REQUIRE_A;     
+    stage = STORING_POINTS;
+    dest_A.zero();
+    dest_B.zero();
+
     return true;
 }
 
-// run - runs the zigzag controller
+// run the zigzag controller
 // should be called at 100hz or more
 void Copter::ModeZigZag::run()
 {
@@ -45,26 +47,92 @@ void Copter::ModeZigZag::run()
         return;
     }
 
-    // manual control activated when point A B is not defined
-    if (stage == REQUIRE_A || stage == REQUIRE_B || stage == MANUAL_REGAIN) {
-        // receive pilot's inputs, do position and attitude control
-        manual_control();
-    } else { 
-        // auto flight
-        // judge if the vehicle has arrived at the current destination
-        // if yes, go to the manual control stage
-        // else, fly to current destination
-        if (has_arr_at_dest()) {  // if the vehicle has arrived at the current destination
+    // auto control
+    if (stage == AUTO) {
+        // if vehicle has reached destination switch to manual control
+        if (reached_destination()) {
+            AP_Notify::events.waypoint_complete = 1;
             stage = MANUAL_REGAIN;
-            loiter_nav->init_target();
-            AP_Notify::events.waypoint_complete = 1;    // play a tone
+            loiter_nav->init_target(wp_nav->get_wp_destination());
         } else {
             auto_control();
         }
     }
+
+    // manual control
+    if (stage == STORING_POINTS || stage == MANUAL_REGAIN) {
+        // receive pilot's inputs, do position and attitude control
+        manual_control();
+    }
 }
 
-// auto_control - guide the vehicle to fly to current destination
+// save current position as A (dest_num = 0) or B (dest_num = 1).  If both A and B have been saved move to the one specified
+void Copter::ModeZigZag::save_or_move_to_destination(uint8_t dest_num)
+{
+    // sanity check
+    if (dest_num > 1) {
+        return;
+    }
+
+    // get current position as an offset from EKF origin
+    const Vector3f curr_pos = inertial_nav.get_position();
+
+    // handle state machine changes
+    switch (stage) {
+
+        case STORING_POINTS:
+            if (dest_num == 0) {
+                // store point A
+                dest_A.x = curr_pos.x;
+                dest_A.y = curr_pos.y;
+                gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: point A stored");
+                copter.Log_Write_Event(DATA_ZIGZAG_STORE_A);
+            } else {
+                // store point B
+                dest_B.x = curr_pos.x;
+                dest_B.y = curr_pos.y;
+                gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: point B stored");
+                copter.Log_Write_Event(DATA_ZIGZAG_STORE_B);
+            }
+            // if both A and B have been stored advance state
+            if (!dest_A.is_zero() && !dest_B.is_zero() && is_positive((dest_B - dest_A).length_squared())) {
+                stage = MANUAL_REGAIN;
+            }
+            break;
+
+        case AUTO:
+        case MANUAL_REGAIN:
+            // A and B have been defined, move vehicle to destination A or B
+            Vector3f next_dest;
+            if (calculate_next_dest(dest_num, next_dest)) {
+                // initialise waypoint controller
+                wp_nav->wp_and_spline_init();
+                if (wp_nav->set_wp_destination(next_dest, false)) {
+                    stage = AUTO;
+                    reach_wp_time_ms = 0;
+                    if (dest_num == 0) {
+                        gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: moving to A");
+                    } else {
+                        gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: moving to B");
+                    }
+                }
+            }
+            break;
+    }
+}
+
+// return manual control to the pilot
+void Copter::ModeZigZag::return_to_manual_control()
+{
+    if (stage == AUTO) {
+        stage = MANUAL_REGAIN;
+        loiter_nav->clear_pilot_desired_acceleration();
+        loiter_nav->init_target();
+        gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: manual control");
+    }
+}
+
+// fly the vehicle to closest point on line perpendicular to dest_A or dest_B
 void Copter::ModeZigZag::auto_control()
 {
     // process pilot's yaw input
@@ -77,10 +145,10 @@ void Copter::ModeZigZag::auto_control()
     // set motors to full range
     motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
-    // run waypoint controller to update xy
+    // run waypoint controller
     copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
 
-    // call z-axis position controller (wpnav should have already updated it's alt target)
+    // call z-axis position controller (wp_nav should have already updated its alt target)
     pos_control->update_z_controller();
 
     // call attitude controller
@@ -120,12 +188,12 @@ void Copter::ModeZigZag::manual_control()
 
     // set motors to full range
     motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
+
     // run loiter controller
     loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
 
     // call attitude controller
-    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(),
-            loiter_nav->get_pitch(), target_yaw_rate);
+    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);
 
     // adjust climb rate using rangefinder
     target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
@@ -135,183 +203,74 @@ void Copter::ModeZigZag::manual_control()
 
     // update altitude target and call position controller
     pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
-    // adjusts target up or down using a climb rate
 
+    // adjusts target up or down using a climb rate
     pos_control->update_z_controller();
 }
 
-// has_arr_at_next_dest - judge if the vehicle is within a small area around the current destination
-bool Copter::ModeZigZag::has_arr_at_dest()
+// return true if vehicle is within a small area around the destination
+bool Copter::ModeZigZag::reached_destination()
 {
-    if (!zigzag_judge_moving.is_keeping_time) {
-        zigzag_judge_moving.is_keeping_time = true;
-        zigzag_judge_moving.last_judge_pos_time = AP_HAL::millis();
-        zigzag_judge_moving.last_pos = inertial_nav.get_position();
+    // check if wp_nav believes it has reached the destination
+    if (!wp_nav->reached_wp_destination()) {
         return false;
     }
-    if ((AP_HAL::millis() - zigzag_judge_moving.last_judge_pos_time) < 1000) {
+
+    // check distance to destination
+    if (wp_nav->get_wp_distance_to_destination() > ZIGZAG_WP_RADIUS_CM) {
         return false;
     }
-    Vector3f cur_pos = inertial_nav.get_position();
-    const float dist_x = cur_pos.x - zigzag_judge_moving.last_pos.x;
-    const float dist_y = cur_pos.y - zigzag_judge_moving.last_pos.y;
-    if ((sq(dist_x) + sq(dist_y)) < ZIGZAG_WP_RADIUS_SQUARED) {
-        return true;
+
+    // wait at least one second
+    uint32_t now = AP_HAL::millis();
+    if (reach_wp_time_ms == 0) {
+        reach_wp_time_ms = now;
     }
-    zigzag_judge_moving.last_judge_pos_time = AP_HAL::millis();
-    zigzag_judge_moving.last_pos = inertial_nav.get_position();
-    return false;
+    return ((now - reach_wp_time_ms) > 1000);
 }
 
-// calculate_next_dest - calculate next destination according to vector A-B and current position
-bool Copter::ModeZigZag::calculate_next_dest(Vector3f& next_dest, RC_Channel::aux_switch_pos_t next_A_or_B) const
+// calculate next destination according to vector A-B and current position
+bool Copter::ModeZigZag::calculate_next_dest(uint8_t dest_num, Vector3f& next_dest) const
 {
-    // calculate difference between A and B - vector AB and its direction
-    Vector2f pos_diff = zigzag_waypoint.B_pos - zigzag_waypoint.A_pos;
-    // get current position
-    Vector3f cur_pos = inertial_nav.get_position();
-    if (!zigzag_is_between_A_and_B) {
-        // if the drone's position is on the side of A or B
-        if (next_A_or_B != zigzag_waypoint.switch_pos_B && next_A_or_B != zigzag_waypoint.switch_pos_A) {
-            return false; // if next_dest not initialised, return false
-        }
-        if (next_A_or_B == zigzag_waypoint.switch_pos_B) {
-            next_dest.x = cur_pos.x + pos_diff.x;
-            next_dest.y = cur_pos.y + pos_diff.y;
-            next_dest.z = cur_pos.z;
-            return true;
-        } 
-        // can only be the case when (next_A_or_B == zigzag_waypoint.switch_pos_A)
-        next_dest.x = cur_pos.x - pos_diff.x;
-        next_dest.y = cur_pos.y - pos_diff.y;
-        next_dest.z = cur_pos.z;
-        return true;
-    }
-    // used to check if the drone is outside A-B scale
-    int8_t next_dir = 1;
-    // if the drone's position is between A and B
-    const Vector2f cur_pos_2d{cur_pos.x, cur_pos.y};
-    const Vector2f AB = zigzag_waypoint.B_pos - zigzag_waypoint.A_pos;
-    const Vector2f P_on_AB = Vector2f::closest_point(cur_pos_2d, zigzag_waypoint.A_pos, zigzag_waypoint.B_pos);
-    float dist_AB = AB.length();
-    float dist_from_AB_squared = (P_on_AB - cur_pos_2d).length_squared();
-    next_dest.z = cur_pos.z;
-    if (is_zero(dist_AB)) {   // protection against division by zero
+    // sanity check dest_num
+    if (dest_num > 1) {
         return false;
     }
-    if (next_A_or_B != zigzag_waypoint.switch_pos_B && next_A_or_B != zigzag_waypoint.switch_pos_A) {
-        return false; // if next_dest not initialised, return false
-    }
-    if (next_A_or_B == zigzag_waypoint.switch_pos_B) {
-        // calculate next B
-        Vector2f pos_diff_BC = cur_pos_2d - zigzag_waypoint.B_pos;
-        if ((pos_diff_BC.x*pos_diff.x + pos_diff_BC.y*pos_diff.y) > 0) {
-            next_dir = -1;
-        }
-        float dist_CB_squared = (cur_pos_2d - zigzag_waypoint.B_pos).length_squared();
-        float dist_BE = sqrtf(dist_CB_squared - dist_from_AB_squared);
-        float dist_ratio = dist_BE / dist_AB;
-        next_dest.x = cur_pos.x + next_dir*dist_ratio*pos_diff.x;
-        next_dest.y = cur_pos.y + next_dir*dist_ratio*pos_diff.y;
-        return true;
-    }
-    // can only be the case when (next_A_or_B == zigzag_waypoint.switch_pos_A)
-    // calculate next A
-    Vector2f pos_diff_AC = cur_pos_2d - zigzag_waypoint.A_pos;
-    if ((pos_diff_AC.x*pos_diff.x + pos_diff_AC.y*pos_diff.y) < 0) {
-        next_dir = -1;
-    }
-    float dist_CA_squared = (cur_pos_2d - zigzag_waypoint.A_pos).length_squared();
-    float dist_AE = sqrtf(dist_CA_squared - dist_from_AB_squared);
-    float dist_ratio = dist_AE / dist_AB;
-    next_dest.x = cur_pos.x - next_dir*dist_ratio*pos_diff.x;
-    next_dest.y = cur_pos.y - next_dir*dist_ratio*pos_diff.y;
-    return true;
-}
 
-// called by ZIGZAG case in RC_Channel.cpp
-// used to record point A, B and give the signal to fly to next destination automatically
-void Copter::ModeZigZag::receive_signal_from_auxsw(RC_Channel::aux_switch_pos_t aux_switch_position)
-{
-    // define point A and B
-    if (stage == REQUIRE_A || stage == REQUIRE_B) {
-        if (aux_switch_position != RC_Channel::aux_switch_pos_t::MIDDLE) {
-            Vector3f cur_pos = inertial_nav.get_position();
-            set_destination(cur_pos, aux_switch_position);
-            return;
-        }
-    } else {      
-        // A and B have been defined
-        if (aux_switch_position != RC_Channel::aux_switch_pos_t::MIDDLE) { // switch position in HIGH or LOW
-            // calculate next point A or B
-            // need to judge if the drone's position is between A and B
-            Vector3f next_dest;
-            if (calculate_next_dest(next_dest, aux_switch_position)) {
-                // initialise waypoint and spline controller
-                wp_nav->wp_and_spline_init();
-                set_destination(next_dest, aux_switch_position);
-                // initialise yaw
-                auto_yaw.set_mode_to_default(false);
-                stage = AUTO;
-                zigzag_is_between_A_and_B = false;
-            }
-        } else {      //switch in middle position, regain the control
-            if (stage == AUTO) {
-                stage = MANUAL_REGAIN;
-                loiter_nav->init_target();
-                zigzag_is_between_A_and_B = true;
-            } else {
-                zigzag_is_between_A_and_B = false;
-            }
-        }
-    }
-}
+    // define start_pos as either A or B depending upon dest_num
+    Vector2f start_pos = dest_num == 0 ? dest_A : dest_B;
 
-// set_destination - sets zigzag mode's target destination
-// Returns true if the fence is enabled and guided waypoint is within the fence
-// else return false if the waypoint is outside the fence
-bool Copter::ModeZigZag::set_destination(const Vector3f& destination, RC_Channel::aux_switch_pos_t aux_switch_position)
-{
+    // calculate vector from A to B
+    Vector2f AB_diff = dest_B - dest_A;
 
-#if AC_FENCE == ENABLED
-    // reject destination if outside the fence
-    Location_Class dest_loc(destination);
-    if (!copter.fence.check_destination_within_fence(dest_loc)) {
-        copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
+    // check distance between A and B
+    if (!is_positive(AB_diff.length_squared())) {
         return false;
     }
-#endif
-    switch (stage) {
-    case REQUIRE_A:
-        // define point A
-        zigzag_waypoint.A_pos.x = destination.x;
-        zigzag_waypoint.A_pos.y = destination.y;
-        zigzag_waypoint.switch_pos_A = aux_switch_position;
-        stage = REQUIRE_B;     // next need to define point B
-        gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: point A stored");
-        copter.Log_Write_Event(DATA_ZIGZAG_STORE_A);
-        return true;
-    case REQUIRE_B:
-        // point B will only be defined after A is defined
-        // if user toggle to the switch position that were previously defined as A
-        // exit the function and do nothing
-        if (aux_switch_position == zigzag_waypoint.switch_pos_A) {
-            return true;
-        }
-        // define point B
-        zigzag_waypoint.B_pos.x = destination.x;
-        zigzag_waypoint.B_pos.y = destination.y;
-        zigzag_waypoint.switch_pos_B = aux_switch_position;
-        stage = MANUAL_REGAIN;  // user is still in manual control until he/she returns the switch again to point A position
-        gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: point B stored");
-        copter.Log_Write_Event(DATA_ZIGZAG_STORE_B);
-        return true;
-    default:
-        // when both A and B are defined and switch in not in middle position, set waypoint destination
-        // no need to check return status because terrain data is not used
-        wp_nav->set_wp_destination(destination, false);
-        return true;
+
+    // get distance from vehicle to start_pos
+    const Vector3f curr_pos = inertial_nav.get_position();
+    const Vector2f curr_pos2d = Vector2f(curr_pos.x, curr_pos.y);
+    Vector2f veh_to_start_pos = curr_pos2d - start_pos;
+
+    // lengthen AB_diff so that it is at least as long as vehicle is from start point
+    // we need to ensure that the lines perpendicular to AB are long enough to reach the vehicle
+    float scalar = 1.0f;
+    if (veh_to_start_pos.length_squared() > AB_diff.length_squared()) {
+        scalar = veh_to_start_pos.length() / AB_diff.length();
     }
 
+    // create a line perpendicular to AB but originating at start_pos
+    Vector2f perp1 = start_pos + Vector2f(-AB_diff[1] * scalar, AB_diff[0] * scalar);
+    Vector2f perp2 = start_pos + Vector2f(AB_diff[1] * scalar, -AB_diff[0] * scalar);
+
+    // find the closest point on the perpendicular line
+    const Vector2f closest2d = Vector2f::closest_point(curr_pos2d, perp1, perp2);
+    next_dest.x = closest2d.x;
+    next_dest.y = closest2d.y;
+    next_dest.z = pos_control->is_active_z() ? pos_control->get_alt_target() : curr_pos.z;
+
+    return true;
 }
+
 #endif // MODE_ZIGZAG_ENABLED == ENABLED