AC_AttitudeControl: Add Landed Gain Backoff

lthall · Apr 24, 2024 · 6e3b783 · 6e3b783
1 parent 1cafe25
commit 6e3b783
Show file tree

Hide file tree

Showing 2 changed files with 52 additions and 0 deletions.
diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
@@ -150,6 +150,33 @@ 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: Roll gain multiplier used when landed
+    // @Description: Roll gain multiplier used when landed
+    // @Range: 0 1
+    // @Increment: 0.01
+    // @Values: 0:No Stabilization, 1.0 Full Stabilization
+    // @User: Advanced
+    AP_GROUPINFO("LAND_R_MULT", 21, AC_AttitudeControl, _land_roll_mult, 1.0),
+
+    // @Param: LAND_P_MULT
+    // @DisplayName: Pitch gain multiplier used when landed
+    // @Description: Pitch gain multiplier used when landed
+    // @Range: 0 1
+    // @Increment: 0.01
+    // @Values: 0:No Stabilization, 1.0 Full Stabilization
+    // @User: Advanced
+    AP_GROUPINFO("LAND_P_MULT", 22, AC_AttitudeControl, _land_pitch_mult, 1.0),
+
+    // @Param: LAND_Y_MULT
+    // @DisplayName: Yaw gain multiplier used when landed
+    // @Description: Yaw gain multiplier used when landed
+    // @Range: 0 1
+    // @Increment: 0.01
+    // @Values: 0:No Stabilization, 1.0 Full Stabilization
+    // @User: Advanced
+    AP_GROUPINFO("LAND_Y_MULT", 23, AC_AttitudeControl, _land_yaw_mult, 1.0),
+
     AP_GROUPEND
 };
 
@@ -204,6 +231,20 @@ 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)
+{
+    const float spool_step = _dt / 0.5;
+    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);
+    }
+    Vector3f scale_mult = VECTORF_111 * _landed_gain_ratio + Vector3f(_land_roll_mult, _land_pitch_mult, _land_yaw_mult) * (1.0 - _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
@@ -148,6 +148,9 @@ class AC_AttitudeControl {
     // 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);
@@ -543,6 +546,14 @@ class AC_AttitudeControl {
     // PD scale used for last loop, used for logging
     Vector3f            _pd_scale_used;
 
+    // ratio of normal gain to landed gain
+    float               _landed_gain_ratio;
+
+    // Controller gain multiplyer to be used when landed
+    AP_Float            _land_roll_mult;
+    AP_Float            _land_pitch_mult;
+    AP_Float            _land_yaw_mult;
+
     // References to external libraries
     const AP_AHRS_View&  _ahrs;
     const AP_MultiCopter &_aparm;