AC_PosControl_Sub: Add new relax_alt_hold_controllers

When changing from manual to alt hold controller it's necessary to hold the I term
since this will help to fix the altitude with vehicles that are not neutral buoyancy

Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>

libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp

@@ -90,3 +90,16 @@ void AC_PosControl_Sub::set_alt_target_from_climb_rate_ff(float climb_rate_cms,
         _vel_desired.z = constrain_float(0.0f, _vel_desired.z-vel_change_limit, _vel_desired.z+vel_change_limit);
     }
 }
+
+/// relax_alt_hold_controllers - set all desired and targets to measured
+void AC_PosControl_Sub::relax_alt_hold_controllers()
+{
+    _pos_target.z = _inav.get_altitude();
+    _vel_desired.z = 0.0f;
+    _flags.use_desvel_ff_z = false;
+    _vel_target.z = _inav.get_velocity_z();
+    _vel_last.z = _inav.get_velocity_z();
+    _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;
+}

libraries/AC_AttitudeControl/AC_PosControl_Sub.h

@@ -33,6 +33,15 @@ public:
     ///     set force_descend to true during landing to allow target to move low enough to slow the motors
     void set_alt_target_from_climb_rate_ff(float climb_rate_cms, float dt, bool force_descend) override;
 
+    /// relax_alt_hold_controllers - set all desired and targets to measured
+    void relax_alt_hold_controllers(float throttle_setting) {
+        AC_PosControl::relax_alt_hold_controllers(throttle_setting);
+    }
+
+    /// relax_alt_hold_controllers - set all desired and targets to measured
+    ///     integrator is not reset
+    void relax_alt_hold_controllers();
+
 private:
     float       _alt_max; // max altitude - should be updated from the main code with altitude limit from fence
     float       _alt_min; // min altitude - should be updated from the main code with altitude limit from fence