From 6152f31c1ee3389e594c61c8024f363684a97e9d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Mon, 18 Mar 2019 14:11:25 -0300 Subject: [PATCH] AC_PosControl_Sub: Add new relax_alt_hold_controllers MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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 --- libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp | 14 ++++++++++++++ libraries/AC_AttitudeControl/AC_PosControl_Sub.h | 9 +++++++++ 2 files changed, 23 insertions(+) diff --git a/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp b/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp index 946727e2e2..2b6821a9ed 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp @@ -88,3 +88,17 @@ 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_desired.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; +} diff --git a/libraries/AC_AttitudeControl/AC_PosControl_Sub.h b/libraries/AC_AttitudeControl/AC_PosControl_Sub.h index c87bc39264..2fc2e0dad9 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl_Sub.h +++ b/libraries/AC_AttitudeControl/AC_PosControl_Sub.h @@ -31,6 +31,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