From 2ab3d0b3b0a337b444e9cce24c567a9fe28598f3 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 23 Jul 2024 17:01:53 +0930 Subject: [PATCH] AC_AttitudeControl: Add Landed Gain Backoff --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 40 +++++++++++++++++++ .../AC_AttitudeControl/AC_AttitudeControl.h | 11 +++++ 2 files changed, 51 insertions(+) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 0465ece670..14d3a1fc7f 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -150,6 +150,27 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = { // @User: Standard AP_GROUPINFO("INPUT_TC", 20, AC_AttitudeControl, _input_tc, AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT), + // @Param: LAND_R_MULT + // @DisplayName: Landed roll gain multiplier + // @Description: Roll gain multiplier active when landed. A factor of 1.0 means no reduction in gain while landed. Reduce this factor to reduce ground oscitation in the roll axis. + // @Range: 0.25 1.0 + // @User: Advanced + AP_GROUPINFO("LAND_R_MULT", 21, AC_AttitudeControl, _land_roll_mult, 1.0), + + // @Param: LAND_P_MULT + // @DisplayName: Landed pitch gain multiplier + // @Description: Pitch gain multiplier active when landed. A factor of 1.0 means no reduction in gain while landed. Reduce this factor to reduce ground oscitation in the pitch axis. + // @Range: 0.25 1.0 + // @User: Advanced + AP_GROUPINFO("LAND_P_MULT", 22, AC_AttitudeControl, _land_pitch_mult, 1.0), + + // @Param: LAND_Y_MULT + // @DisplayName: Landed yaw gain multiplier + // @Description: Yaw gain multiplier active when landed. A factor of 1.0 means no reduction in gain while landed. Reduce this factor to reduce ground oscitation in the yaw axis. + // @Range: 0.25 1.0 + // @User: Advanced + AP_GROUPINFO("LAND_Y_MULT", 23, AC_AttitudeControl, _land_yaw_mult, 1.0), + AP_GROUPEND }; @@ -204,6 +225,25 @@ void AC_AttitudeControl::reset_rate_controller_I_terms_smoothly() get_rate_yaw_pid().relax_integrator(0.0, _dt, AC_ATTITUDE_RATE_RELAX_TC); } +// Reduce attitude control gains while landed to stop ground resonance +void AC_AttitudeControl::landed_gain_reduction(bool landed) +{ + if (is_positive(_input_tc)) { + // use 2.0 x tc to match the response time to 86% commanded + const float spool_step = _dt / (2.0 * _input_tc); + if (landed) { + _landed_gain_ratio = MIN(1.0, _landed_gain_ratio + spool_step); + } else { + _landed_gain_ratio = MAX(0.0, _landed_gain_ratio - spool_step); + } + } else { + _landed_gain_ratio = landed ? 1.0 : 0.0; + } + Vector3f scale_mult = VECTORF_111 * (1.0 - _landed_gain_ratio) + Vector3f(_land_roll_mult, _land_pitch_mult, _land_yaw_mult) * _landed_gain_ratio; + set_PD_scale_mult(scale_mult); + set_angle_P_scale_mult(scale_mult); +} + // The attitude controller works around the concept of the desired attitude, target attitude // and measured attitude. The desired attitude is the attitude input into the attitude controller // that expresses where the higher level code would like the aircraft to move to. The target attitude is moved diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 1883546c56..b396828258 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -163,6 +163,9 @@ public: // reset rate controller I terms smoothly to zero in 0.5 seconds void reset_rate_controller_I_terms_smoothly(); + // Reduce attitude control gains while landed to stop ground resonance + void landed_gain_reduction(bool landed); + // Sets attitude target to vehicle attitude and sets all rates to zero // If reset_rate is false rates are not reset to allow the rate controllers to run void reset_target_and_rate(bool reset_rate = true); @@ -479,6 +482,11 @@ protected: // rate controller input smoothing time constant AP_Float _input_tc; + // Controller gain multiplyer to be used when landed + AP_Float _land_roll_mult; + AP_Float _land_pitch_mult; + AP_Float _land_yaw_mult; + // Intersampling period in seconds float _dt; @@ -561,6 +569,9 @@ protected: // PD scale used for last loop, used for logging Vector3f _pd_scale_used; + // ratio of normal gain to landed gain + float _landed_gain_ratio; + // References to external libraries const AP_AHRS_View& _ahrs; const AP_MultiCopter &_aparm;