From 21bfa0dc85403ff0c3b5421f00338728e370f7ce Mon Sep 17 00:00:00 2001
From: Leonard Hall <leonardthall@gmail.com>
Date: Thu, 20 Jun 2024 13:06:47 +0930
Subject: [PATCH] AC_AttitudeControl: Add Landed Gain Backoff

---
 .../AC_AttitudeControl/AC_AttitudeControl.cpp | 36 +++++++++++++++++++
 .../AC_AttitudeControl/AC_AttitudeControl.h   | 11 ++++++
 2 files changed, 47 insertions(+)

diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
index c68e143a112fb1..91f84c11670660 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: Roll gain multiplier used when landed
+    // @Description: Roll gain multiplier used when landed
+    // @Range: 0 1
+    // @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
+    // @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
+    // @User: Advanced
+    AP_GROUPINFO("LAND_Y_MULT", 23, AC_AttitudeControl, _land_yaw_mult, 1.0),
+
     AP_GROUPEND
 };
 
@@ -204,6 +225,21 @@ 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)
+{
+    // 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);
+    }
+    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
index 29ec8eae60682a..833792a51e4657 100644
--- 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);
@@ -461,6 +464,11 @@ class AC_AttitudeControl {
     // 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;
 
@@ -543,6 +551,9 @@ 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;
+
     // References to external libraries
     const AP_AHRS_View&  _ahrs;
     const AP_MultiCopter &_aparm;