Copter: zigzag mode gets terrain following support

ArduCopter/Attitude.cpp

@@ -187,6 +187,25 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate)
 #endif
 }
 
+// get surfacing tracking alt
+// returns true if there is a valid target
+bool Copter::get_surface_tracking_target_alt_cm(float &target_alt_cm) const
+{
+    // check target has been updated recently
+    if (AP_HAL::millis() - surface_tracking.last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) {
+        return false;
+    }
+    target_alt_cm = surface_tracking.target_alt_cm;
+    return true;
+}
+
+// set surface tracking target altitude
+void Copter::set_surface_tracking_target_alt_cm(float target_alt_cm)
+{
+    surface_tracking.target_alt_cm = target_alt_cm;
+    surface_tracking.last_update_ms = AP_HAL::millis();
+}
+
 // get target climb rate reduced to avoid obstacles and altitude fence
 float Copter::get_avoidance_adjusted_climbrate(float target_rate)
 {

ArduCopter/Copter.h

@@ -622,6 +622,8 @@ private:
     float get_pilot_desired_climb_rate(float throttle_control);
     float get_non_takeoff_throttle();
     float get_surface_tracking_climb_rate(int16_t target_rate);
+    bool get_surface_tracking_target_alt_cm(float &target_alt_cm) const;
+    void set_surface_tracking_target_alt_cm(float target_alt_cm);
     float get_avoidance_adjusted_climbrate(float target_rate);
     void set_accel_throttle_I_from_pilot_throttle();
     void rotate_body_frame_to_NE(float &x, float &y);

ArduCopter/RC_Channel.cpp

@@ -522,7 +522,7 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw
                         copter.mode_zigzag.save_or_move_to_destination(0);
                         break;
                     case MIDDLE:
-                        copter.mode_zigzag.return_to_manual_control();
+                        copter.mode_zigzag.return_to_manual_control(false);
                         break;
                     case HIGH:
                         copter.mode_zigzag.save_or_move_to_destination(1);

ArduCopter/mode.cpp

@@ -693,6 +693,16 @@ float Copter::Mode::get_surface_tracking_climb_rate(int16_t target_rate)
     return copter.get_surface_tracking_climb_rate(target_rate);
 }
 
+bool Copter::Mode::get_surface_tracking_target_alt_cm(float &target_alt_cm) const
+{
+    return copter.get_surface_tracking_target_alt_cm(target_alt_cm);
+}
+
+void Copter::Mode::set_surface_tracking_target_alt_cm(float target_alt_cm)
+{
+   copter.set_surface_tracking_target_alt_cm(target_alt_cm);
+}
+
 float Copter::Mode::get_pilot_desired_yaw_rate(int16_t stick_angle)
 {
     return copter.get_pilot_desired_yaw_rate(stick_angle);

ArduCopter/mode.h

@@ -198,6 +198,8 @@ protected:
     // these are candidates for moving into the Mode base
     // class.
     float get_surface_tracking_climb_rate(int16_t target_rate);
+    bool get_surface_tracking_target_alt_cm(float &target_alt_cm) const;
+    void set_surface_tracking_target_alt_cm(float target_alt_cm);
     float get_pilot_desired_yaw_rate(int16_t stick_angle);
     float get_pilot_desired_climb_rate(float throttle_control);
     float get_pilot_desired_throttle() const;
@@ -1206,7 +1208,7 @@ public:
     void save_or_move_to_destination(uint8_t dest_num);
 
     // return manual control to the pilot
-    void return_to_manual_control();
+    void return_to_manual_control(bool maintain_target);
 
 protected:
 
@@ -1218,7 +1220,7 @@ private:
     void auto_control();
     void manual_control();
     bool reached_destination();
-    bool calculate_next_dest(uint8_t position_num, Vector3f& next_dest) const;
+    bool calculate_next_dest(uint8_t position_num, bool use_wpnav_alt, Vector3f& next_dest, bool& terrain_alt) const;
 
     Vector2f dest_A;    // in NEU frame in cm relative to ekf origin
     Vector2f dest_B;    // in NEU frame in cm relative to ekf origin

ArduCopter/mode_zigzag.cpp

@@ -48,8 +48,7 @@ void Copter::ModeZigZag::run()
         // if vehicle has reached destination switch to manual control
         if (reached_destination()) {
             AP_Notify::events.waypoint_complete = 1;
-            stage = MANUAL_REGAIN;
+            return_to_manual_control(true);
-            loiter_nav->init_target(wp_nav->get_wp_destination());
         } else {
             auto_control();
         }
@@ -100,10 +99,10 @@ void Copter::ModeZigZag::save_or_move_to_destination(uint8_t dest_num)
         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)) {
+            bool terr_alt;
-                // initialise waypoint controller
+            if (calculate_next_dest(dest_num, stage == AUTO, next_dest, terr_alt)) {
                 wp_nav->wp_and_spline_init();
-                if (wp_nav->set_wp_destination(next_dest, false)) {
+                if (wp_nav->set_wp_destination(next_dest, terr_alt)) {
                     stage = AUTO;
                     reach_wp_time_ms = 0;
                     if (dest_num == 0) {
@@ -118,12 +117,16 @@ void Copter::ModeZigZag::save_or_move_to_destination(uint8_t dest_num)
 }
 
 // return manual control to the pilot
-void Copter::ModeZigZag::return_to_manual_control()
+void Copter::ModeZigZag::return_to_manual_control(bool maintain_target)
 {
     if (stage == AUTO) {
         stage = MANUAL_REGAIN;
         loiter_nav->clear_pilot_desired_acceleration();
-        loiter_nav->init_target();
+        const Vector3f wp_dest = wp_nav->get_wp_destination();
+        loiter_nav->init_target(wp_dest);
+        if (maintain_target && wp_nav->origin_and_destination_are_terrain_alt()) {
+            set_surface_tracking_target_alt_cm(wp_dest.z);
+        }
         gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: manual control");
     }
 }
@@ -142,14 +145,19 @@ void Copter::ModeZigZag::auto_control()
     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
     // run waypoint controller
-    copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
+    const bool wpnav_ok = wp_nav->update_wpnav();
 
     // call z-axis position controller (wp_nav should have already updated its alt target)
     pos_control->update_z_controller();
 
     // call attitude controller
     // roll & pitch from waypoint controller, yaw rate from pilot
-    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);        
+    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
+
+    // if wpnav failed (because of lack of terrain data) switch back to pilot control for next iteration
+    if (!wpnav_ok) {
+        return_to_manual_control(false);
+    }
 }
 
 // manual_control - process manual control
@@ -226,7 +234,9 @@ bool Copter::ModeZigZag::reached_destination()
 }
 
 // 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
+// use_wpnav_alt should be true if waypoint controller's altitude target should be used, false for position control or current altitude target
+// terrain_alt is returned as true if the next_dest should be considered a terrain alt
+bool Copter::ModeZigZag::calculate_next_dest(uint8_t dest_num, bool use_wpnav_alt, Vector3f& next_dest, bool& terrain_alt) const
 {
     // sanity check dest_num
     if (dest_num > 1) {
@@ -264,7 +274,22 @@ bool Copter::ModeZigZag::calculate_next_dest(uint8_t dest_num, Vector3f& next_de
     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;
+
+    if (use_wpnav_alt) {
+        // get altitude target from waypoint controller
+        terrain_alt = wp_nav->origin_and_destination_are_terrain_alt();
+        next_dest.z = wp_nav->get_wp_destination().z;
+    } else {
+        // if we have a downward facing range finder then use terrain altitude targets
+        terrain_alt = copter.rangefinder_alt_ok() && wp_nav->rangefinder_used();
+        if (terrain_alt) {
+            if (!get_surface_tracking_target_alt_cm(next_dest.z)) {
+                next_dest.z = copter.rangefinder_state.alt_cm_filt.get();
+            }
+        } else {
+            next_dest.z = pos_control->is_active_z() ? pos_control->get_alt_target() : curr_pos.z;
+        }
+    }
 
     return true;
 }