AC_PosControl: use lower alt gain during ground effect mode

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -232,7 +232,11 @@ void AC_PosControl::update_z_controller()
     calc_leash_length_z();
 
     // call position controller
-    pos_to_rate_z();
+    if(_gnd_effect_mode) {
+        gnd_effect_pos_to_rate_z();
+    } else {
+        pos_to_rate_z();
+    }
 }
 
 /// calc_leash_length - calculates the vertical leash lengths from maximum speed, acceleration
@@ -246,6 +250,43 @@ void AC_PosControl::calc_leash_length_z()
     }
 }
 
+void AC_PosControl::update_gnd_effect_targets(float dt) {
+    _pos_error.z = _pos_target.z - _gnd_effect_pos_target_z;
+
+    // clear position limit flags
+    _limit.pos_up = false;
+    _limit.pos_down = false;
+
+    // do not let target altitude get too far from current altitude
+    if (_pos_error.z > _leash_up_z) {
+        _pos_target.z = _gnd_effect_pos_target_z + _leash_up_z;
+        _pos_error.z = _leash_up_z;
+        _limit.pos_up = true;
+    }
+    if (_pos_error.z < -_leash_down_z) {
+        _pos_target.z = _gnd_effect_pos_target_z - _leash_down_z;
+        _pos_error.z = -_leash_down_z;
+        _limit.pos_down = true;
+    }
+
+    _gnd_effect_vel_desired_z = AC_AttitudeControl::sqrt_controller(_pos_error.z, _p_alt_pos.kP(), _accel_z_cms);
+    _gnd_effect_pos_error_z = _gnd_effect_pos_target_z - _inav.get_altitude();
+
+    _gnd_effect_pos_target_z += _gnd_effect_vel_desired_z * dt;
+}
+
+void AC_PosControl::gnd_effect_pos_to_rate_z() {
+    // update gnd effect pos target
+    update_gnd_effect_targets(_dt);
+
+    // calculate _vel_target.z using from _pos_error.z using sqrt controller
+    _vel_target.z = AC_AttitudeControl::sqrt_controller(_gnd_effect_pos_error_z, _p_alt_pos.kP()*POSCONTROL_GNDEFFECT_GAIN, _accel_z_cms);
+    _vel_target.z += _gnd_effect_vel_desired_z;
+
+    // call rate based throttle controller which will update accel based throttle controller targets
+    rate_to_accel_z();
+}
+
 // pos_to_rate_z - position to rate controller for Z axis
 // calculates desired rate in earth-frame z axis and passes to rate controller
 // vel_up_max, vel_down_max should have already been set before calling this method
@@ -875,3 +916,27 @@ float AC_PosControl::calc_leash_length(float speed_cms, float accel_cms, float k
 
     return leash_length;
 }
+
+// set the height controller to use method that relies more on forward path velocity and less on height error
+// this reduces the sensitivity to baro noise and transient errors in ground effect
+void AC_PosControl::setGndEffectMode(bool gndEffectMode)
+{
+    if(_gnd_effect_mode != gndEffectMode) {
+        if(gndEffectMode) {
+            // turning feedforward on, initialize
+            float curr_alt = _inav.get_altitude();
+            _gnd_effect_pos_target_z = curr_alt;
+            update_gnd_effect_targets(0.0f);
+        } else {
+            // turning feedforward off - adjust position target such that velocity demand stays constant:
+            // sqrt_controller_gnd(err_gnd)+vel_desired = sqrt_controller_air(err_air)
+            // find err_gnd such that this equality is met:
+            // err_air = sqrt_controller_air_inv(sqrt_controller_gnd(err_gnd)+vel_desired)
+            float curr_alt = _inav.get_altitude();
+            float prev_fb_vel_target = AC_AttitudeControl::sqrt_controller(_gnd_effect_pos_error_z, _p_alt_pos.kP()*POSCONTROL_GNDEFFECT_GAIN, _accel_z_cms);
+            float new_pos_error = AC_AttitudeControl::inverse_sqrt_controller(prev_fb_vel_target+_gnd_effect_vel_desired_z, _p_alt_pos.kP(), _accel_z_cms);
+            _pos_target.z = new_pos_error + curr_alt;
+        }
+    }
+    _gnd_effect_mode = gndEffectMode;
+}

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -42,6 +42,8 @@
 #define POSCONTROL_VEL_ERROR_CUTOFF_FREQ        4.0     // 4hz low-pass filter on velocity error
 #define POSCONTROL_ACCEL_ERROR_CUTOFF_FREQ      20.0     // 2hz low-pass filter on accel error
 
+#define POSCONTROL_GNDEFFECT_GAIN               0.2f
+
 class AC_PosControl
 {
 public:
@@ -199,6 +201,10 @@ class AC_PosControl
     ///     when update_vel_controller_xyz is next called the position target is moved based on the desired velocity
     void set_desired_velocity(const Vector3f &des_vel) { _vel_desired = des_vel; freeze_ff_xy(); }
 
+    /// set the height controller to use method that relies more on forward path velocity and less on height error
+    /// this reduces the sensitivity to baro noise and transient errors in ground effect
+    void setGndEffectMode(bool gndEffectMode);
+
     /// freeze_ff_z - used to stop the feed forward being calculated during a known discontinuity
     void freeze_ff_z() { _flags.freeze_ff_z = true; }
 
@@ -295,6 +301,7 @@ class AC_PosControl
     //          set_target_to_stopping_point_z
     //          init_takeoff
     void pos_to_rate_z();
+    void gnd_effect_pos_to_rate_z();
 
     // rate_to_accel_z - calculates desired accel required to achieve the velocity target
     void rate_to_accel_z();
@@ -327,6 +334,8 @@ class AC_PosControl
     /// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration and position kP gain
     float calc_leash_length(float speed_cms, float accel_cms, float kP) const;
 
+    void update_gnd_effect_targets(float dt);
+
     // references to inertial nav and ahrs libraries
     const AP_AHRS&              _ahrs;
     const AP_InertialNav&       _inav;
@@ -376,5 +385,9 @@ class AC_PosControl
     float       _distance_to_target;    // distance to position target - for reporting only
     LowPassFilterFloat _vel_error_filter;   // low-pass-filter on z-axis velocity error
     LowPassFilterFloat _accel_error_filter; // low-pass-filter on z-axis accelerometer error
+    bool        _gnd_effect_mode;         // true when a height control relying more on forward path velocity is being used to reduce ground effect coupling
+    float       _gnd_effect_pos_target_z;
+    float       _gnd_effect_pos_error_z;
+    float       _gnd_effect_vel_desired_z;
 };
 #endif	// AC_POSCONTROL_H