AC_PosControl: add relax Alt Hold controllers

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -160,6 +160,20 @@ void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float d
     }
 }
 
+/// relax_alt_hold_controllers - set all desired and targets to measured
+void AC_PosControl::relax_alt_hold_controllers(float throttle_setting)
+{
+    _pos_target.z = _inav.get_altitude();
+    _vel_desired.z = 0.0f;
+    _vel_target.z= _inav.get_velocity_z();
+    _vel_last.z = _inav.get_velocity_z();
+    _accel_feedforward.z = 0.0f;
+    _accel_last_z_cms = 0.0f;
+    _accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;
+    _flags.reset_accel_to_throttle = true;
+    _pid_accel_z.set_integrator(throttle_setting);
+}
+
 // get_alt_error - returns altitude error in cm
 float AC_PosControl::get_alt_error() const
 {

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -126,6 +126,9 @@ public:
     /// set_alt_target_to_current_alt - set altitude target to current altitude
     void set_alt_target_to_current_alt() { _pos_target.z = _inav.get_altitude(); }
 
+    /// relax_alt_hold_controllers - set all desired and targets to measured
+    void relax_alt_hold_controllers(float throttle_setting);
+
     /// get_alt_target, get_desired_alt - get desired altitude (in cm above home) from loiter or wp controller which should be fed into throttle controller
     /// To-Do: remove one of the two functions below
     float get_alt_target() const { return _pos_target.z; }