Copter: Separate landing and terrain following.

ArduCopter/Copter.h

@@ -268,8 +268,9 @@ class Copter : public AP_Vehicle {
 
     class SurfaceTracking {
     public:
-        // get desired climb rate (in cm/s) to achieve surface tracking
-        float adjust_climb_rate(float target_rate);
+        // update_surface_offset - manages the vertical offset of the position controller to follow the
+        //   measured ground or ceiling level measured using the range finder.
+        void update_surface_offset();
 
         // get/set target altitude (in cm) above ground
         bool get_target_alt_cm(float &target_alt_cm) const;
@@ -291,7 +292,6 @@ class Copter : public AP_Vehicle {
 
     private:
         Surface surface = Surface::GROUND;
-        float target_dist_cm;       // desired distance in cm from ground or ceiling
         uint32_t last_update_ms;    // system time of last update to target_alt_cm
         uint32_t last_glitch_cleared_ms;    // system time of last handle glitch recovery
         bool valid_for_logging;     // true if target_alt_cm is valid for logging

ArduCopter/mode.cpp

@@ -593,7 +593,7 @@ void Mode::land_run_vertical_control(bool pause_descent)
     }
 
     // update altitude target and call position controller
-    pos_control->set_pos_target_z_from_climb_rate_cm(cmb_rate, ignore_descent_limit);
+    pos_control->land_at_climb_rate_cm(cmb_rate, ignore_descent_limit);
     pos_control->update_z_controller();
 }
 

ArduCopter/mode_althold.cpp

@@ -84,13 +84,14 @@ void ModeAltHold::run()
         copter.avoid.adjust_roll_pitch(target_roll, target_pitch, copter.aparm.angle_max);
 #endif
 
-        // adjust climb rate using rangefinder
-        target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate);
-
         // get avoidance adjusted climb rate
         target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
 
-        pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false);
+        // update the vertical offset based on the surface measurement
+        copter.surface_tracking.update_surface_offset();
+
+        // Send the commanded climb rate to the position controller
+        pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);
         break;
     }
 

ArduCopter/mode_auto.cpp

@@ -1018,7 +1018,12 @@ void ModeAuto::loiter_to_alt_run()
     // get avoidance adjusted climb rate
     target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
 
-    pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false);
+    // update the vertical offset based on the surface measurement
+    copter.surface_tracking.update_surface_offset();
+
+    // Send the commanded climb rate to the position controller
+    pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);
+
     pos_control->update_z_controller();
 }
 

ArduCopter/mode_autorotate.cpp

@@ -252,7 +252,7 @@ void ModeAutorotate::run()
             _pitch_target -= _target_pitch_adjust*G_Dt;
         }
         // Set position controller
-        pos_control->set_pos_target_z_from_climb_rate_cm(_desired_v_z, false);
+        pos_control->set_pos_target_z_from_climb_rate_cm(_desired_v_z);
 
         // Update controllers
         pos_control->update_z_controller();

ArduCopter/mode_brake.cpp

@@ -59,7 +59,7 @@ void ModeBrake::run()
     // call attitude controller
     attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), 0.0f);
 
-    pos_control->set_pos_target_z_from_climb_rate_cm(0.0f, false);
+    pos_control->set_pos_target_z_from_climb_rate_cm(0.0f);
     pos_control->update_z_controller();
 
     if (_timeout_ms != 0 && millis()-_timeout_start >= _timeout_ms) {

ArduCopter/mode_circle.cpp

@@ -89,11 +89,6 @@ void ModeCircle::run()
 
     // get pilot desired climb rate (or zero if in radio failsafe)
     float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
-    // adjust climb rate using rangefinder
-    if (copter.rangefinder_alt_ok()) {
-        // if rangefinder is ok, use surface tracking
-        target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate);
-    }
 
     // if not armed set throttle to zero and exit immediately
     if (is_disarmed_or_landed()) {
@@ -104,7 +99,9 @@ void ModeCircle::run()
     // set motors to full range
     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
-    // run circle controller
+    // update the vertical offset based on the surface measurement
+    copter.surface_tracking.update_surface_offset();
+
     copter.failsafe_terrain_set_status(copter.circle_nav->update(target_climb_rate));
 
     // call attitude controller

ArduCopter/mode_flowhold.cpp

@@ -297,13 +297,14 @@ void ModeFlowHold::run()
     case AltHold_Flying:
         copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
-        // adjust climb rate using rangefinder
-        target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate);
-
         // get avoidance adjusted climb rate
         target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
 
-        copter.pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false);
+        // update the vertical offset based on the surface measurement
+        copter.surface_tracking.update_surface_offset();
+
+        // Send the commanded climb rate to the position controller
+        pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);
         break;
     }
 

ArduCopter/mode_guided.cpp

@@ -879,7 +879,7 @@ void ModeGuided::angle_control_run()
     if (guided_angle_state.use_thrust) {
         attitude_control->set_throttle_out(guided_angle_state.thrust, true, copter.g.throttle_filt);
     } else {
-        pos_control->set_pos_target_z_from_climb_rate_cm(climb_rate_cms, false);
+        pos_control->set_pos_target_z_from_climb_rate_cm(climb_rate_cms);
         pos_control->update_z_controller();
     }
 }

ArduCopter/mode_loiter.cpp

@@ -171,13 +171,14 @@ void ModeLoiter::run()
         // call attitude controller
         attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate);
 
-        // adjust climb rate using rangefinder
-        target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate);
-
         // get avoidance adjusted climb rate
         target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
 
-        pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false);
+        // update the vertical offset based on the surface measurement
+        copter.surface_tracking.update_surface_offset();
+
+        // Send the commanded climb rate to the position controller
+        pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);
         break;
     }
 

ArduCopter/mode_poshold.cpp

@@ -158,16 +158,14 @@ void ModePosHold::run()
     case AltHold_Flying:
         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
-        // adjust climb rate using rangefinder
-        if (copter.rangefinder_alt_ok()) {
-            // if rangefinder is ok, use surface tracking
-            target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate);
-        }
-
         // get avoidance adjusted climb rate
         target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
 
-        pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false);
+        // update the vertical offset based on the surface measurement
+        copter.surface_tracking.update_surface_offset();
+
+        // Send the commanded climb rate to the position controller
+        pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);
         break;
     }
 

ArduCopter/mode_sport.cpp

@@ -105,13 +105,14 @@ void ModeSport::run()
     case AltHold_Flying:
         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
-        // adjust climb rate using rangefinder
-        target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate);
-
         // get avoidance adjusted climb rate
         target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
 
-        pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false);
+        // update the vertical offset based on the surface measurement
+        copter.surface_tracking.update_surface_offset();
+
+        // Send the commanded climb rate to the position controller
+        pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);
         break;
     }
 

ArduCopter/mode_throw.cpp

@@ -173,7 +173,7 @@ void ModeThrow::run()
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
 
         // call height controller
-        pos_control->set_pos_target_z_from_climb_rate_cm(0.0f, false);
+        pos_control->set_pos_target_z_from_climb_rate_cm(0.0f);
         pos_control->update_z_controller();
 
         break;
@@ -193,7 +193,7 @@ void ModeThrow::run()
         attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), 0.0f);
 
         // call height controller
-        pos_control->set_pos_target_z_from_climb_rate_cm(0.0f, false);
+        pos_control->set_pos_target_z_from_climb_rate_cm(0.0f);
         pos_control->update_z_controller();
 
         break;

ArduCopter/mode_zigzag.cpp

@@ -376,13 +376,14 @@ void ModeZigZag::manual_control()
         // call attitude controller
         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 = copter.surface_tracking.adjust_climb_rate(target_climb_rate);
-
         // get avoidance adjusted climb rate
         target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
 
-        pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false);
+        // update the vertical offset based on the surface measurement
+        copter.surface_tracking.update_surface_offset();
+
+        // Send the commanded climb rate to the position controller
+        pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);
         break;
     }
 

ArduCopter/surface_tracking.cpp

@@ -1,67 +1,46 @@
 #include "Copter.h"
 
-// adjust_climb_rate - hold copter at the desired distance above the
-//      ground; returns climb rate (in cm/s) which should be passed to
-//      the position controller
-float Copter::SurfaceTracking::adjust_climb_rate(float target_rate)
+// update_surface_offset - manages the vertical offset of the position controller to follow the measured ground or ceiling
+//   level measured using the range finder.
+void Copter::SurfaceTracking::update_surface_offset()
 {
+    float offset_cm = 0.0;
 #if RANGEFINDER_ENABLED == ENABLED
     // check tracking state and that range finders are healthy
-    if ((surface == Surface::NONE) ||
-        ((surface == Surface::GROUND) && (!copter.rangefinder_alt_ok() || (copter.rangefinder_state.glitch_count != 0))) ||
-        ((surface == Surface::CEILING) && !copter.rangefinder_up_ok()) || (copter.rangefinder_up_state.glitch_count != 0)) {
-        return target_rate;
-    }
-
-    // calculate current ekf based altitude error
-    const float current_alt_error = copter.pos_control->get_pos_target_z_cm() - copter.inertial_nav.get_altitude();
-
-    // init based on tracking direction/state
-    RangeFinderState &rf_state = (surface == Surface::GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state;
-    const float dir = (surface == Surface::GROUND) ? 1.0f : -1.0f;
-
-    // reset target altitude if this controller has just been engaged
-    // target has been changed between upwards vs downwards
-    // or glitch has cleared
-    const uint32_t now = millis();
-    if ((now - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) ||
-        reset_target ||
-        (last_glitch_cleared_ms != rf_state.glitch_cleared_ms)) {
-        target_dist_cm = rf_state.alt_cm + (dir * current_alt_error);
-        reset_target = false;
-        last_glitch_cleared_ms = rf_state.glitch_cleared_ms;\
-    }
-    last_update_ms = now;
+    if (((surface == Surface::GROUND) && copter.rangefinder_alt_ok() && (copter.rangefinder_state.glitch_count == 0)) ||
+        ((surface == Surface::CEILING) && copter.rangefinder_up_ok() && (copter.rangefinder_up_state.glitch_count == 0))) {
 
-    // adjust rangefinder target alt if motors have not hit their limits
-    if ((target_rate<0 && !copter.motors->limit.throttle_lower) || (target_rate>0 && !copter.motors->limit.throttle_upper)) {
-        target_dist_cm += dir * target_rate * copter.G_Dt;
-    }
-    valid_for_logging = true;
+        // init based on tracking direction/state
+        RangeFinderState &rf_state = (surface == Surface::GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state;
+        const float dir = (surface == Surface::GROUND) ? 1.0f : -1.0f;
+        offset_cm = copter.inertial_nav.get_altitude() - dir * rf_state.alt_cm;
 
-#if AC_AVOID_ENABLED == ENABLED
-    // upward facing terrain following never gets closer than avoidance margin
-    if (surface == Surface::CEILING) {
-        const float margin_cm = copter.avoid.enabled() ? copter.avoid.get_margin() * 100.0f : 0.0f;
-        target_dist_cm = MAX(target_dist_cm, margin_cm);
+        // reset target altitude if this controller has just been engaged
+        // target has been changed between upwards vs downwards
+        // or glitch has cleared
+        const uint32_t now = millis();
+        if ((now - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) ||
+            reset_target ||
+            (last_glitch_cleared_ms != rf_state.glitch_cleared_ms)) {
+            copter.pos_control->set_pos_offset_z_cm(offset_cm);
+            reset_target = false;
+            last_glitch_cleared_ms = rf_state.glitch_cleared_ms;
+        }
+        last_update_ms = now;
+        valid_for_logging = true;
+    } else {
+        copter.pos_control->set_pos_offset_z_cm(offset_cm);
     }
-#endif
-
-    // calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations)
-    const float distance_error = (target_dist_cm - rf_state.alt_cm) - (dir * current_alt_error);
-    float velocity_correction = dir * distance_error * copter.g.rangefinder_gain;
-    velocity_correction = constrain_float(velocity_correction, -SURFACE_TRACKING_VELZ_MAX, SURFACE_TRACKING_VELZ_MAX);
-
-    // return combined pilot climb rate + rate to correct rangefinder alt error
-    return (target_rate + velocity_correction);
 #else
-    return target_rate;
+    copter.pos_control->set_pos_offset_z_cm(offset_cm);
 #endif
+    copter.pos_control->set_pos_offset_target_z_cm(offset_cm);
 }
 
+
 // get target altitude (in cm) above ground
 // returns true if there is a valid target
-bool Copter::SurfaceTracking::get_target_alt_cm(float &_target_alt_cm) const
+bool Copter::SurfaceTracking::get_target_alt_cm(float &target_alt_cm) const
 {
     // fail if we are not tracking downwards
     if (surface != Surface::GROUND) {
@@ -71,7 +50,7 @@ bool Copter::SurfaceTracking::get_target_alt_cm(float &_target_alt_cm) const
     if (AP_HAL::millis() - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) {
         return false;
     }
-    _target_alt_cm = target_dist_cm;
+    target_alt_cm = (copter.pos_control->get_pos_target_z_cm() - copter.pos_control->get_pos_offset_z_cm());
     return true;
 }
 
@@ -82,7 +61,7 @@ void Copter::SurfaceTracking::set_target_alt_cm(float _target_alt_cm)
     if (surface != Surface::GROUND) {
         return;
     }
-    target_dist_cm = _target_alt_cm;
+    copter.pos_control->set_pos_offset_z_cm(copter.inertial_nav.get_altitude() - _target_alt_cm);
     last_update_ms = AP_HAL::millis();
 }
 
@@ -92,7 +71,8 @@ bool Copter::SurfaceTracking::get_target_dist_for_logging(float &target_dist) co
         return false;
     }
 
-    target_dist = target_dist_cm * 0.01f;
+    const float dir = (surface == Surface::GROUND) ? 1.0f : -1.0f;
+    target_dist = dir * (copter.pos_control->get_pos_target_z_cm() - copter.pos_control->get_pos_offset_z_cm()) * 0.01f;
     return true;
 }