AC_AttitudeControl: add ATC_ANG_LIM_TC parameter

This allow adjusting the reponse to limit lean angles to reduce altitude loss

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -87,6 +87,13 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = {
     // @User: Standard
     AP_SUBGROUPINFO(_p_angle_yaw, "ANG_YAW_", 15, AC_AttitudeControl, AC_P),
 
+    // @Param: ANG_LIM_TC
+    // @DisplayName: Angle Limit (to maintain altitude) Time Constant
+    // @Description: Angle Limit (to maintain altitude) Time Constant
+    // @Range: 0.5 10.0
+    // @User: Advanced
+    AP_GROUPINFO("ANG_LIM_TC", 16, AC_AttitudeControl, _angle_limit_tc, AC_ATTITUDE_CONTROL_ANGLE_LIMIT_TC_DEFAULT),
+
     AP_GROUPEND
 };
 

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -40,8 +40,8 @@
 
 #define AC_ATTITUDE_CONTROL_RATE_BF_FF_DEFAULT          1       // body-frame rate feedforward enabled by default
 
-#define AC_ATTITUDE_CONTROL_ALTHOLD_LEAN_ANGLE_TC_DEFAULT   1.0f    // Time constant used to limit lean angle so that vehicle does not lose altitude
-#define AC_ATTITUDE_CONTROL_ALTHOLD_LEAN_ANGLE_THROTTLE_MAX 0.8f    // Max throttle used to limit lean angle so that vehicle does not lose altitude
+#define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_TC_DEFAULT      1.0f    // Time constant used to limit lean angle so that vehicle does not lose altitude
+#define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX    0.8f    // Max throttle used to limit lean angle so that vehicle does not lose altitude
 
 #define AC_ATTITUDE_CONTROL_MIN_DEFAULT   0.1f    // minimum throttle mix
 #define AC_ATTITUDE_CONTROL_MID_DEFAULT   0.5f    // manual throttle mix
@@ -316,6 +316,9 @@ protected:
     AC_P                _p_angle_pitch;
     AC_P                _p_angle_yaw;
 
+    // Angle limit time constant (to maintain altitude)
+    AP_Float            _angle_limit_tc;
+
     // Intersampling period in seconds
     float               _dt;
 

libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp

@@ -235,8 +235,8 @@ void AC_AttitudeControl_Heli::rate_controller_run()
 // Update Alt_Hold angle maximum
 void AC_AttitudeControl_Heli::update_althold_lean_angle_max(float throttle_in)
 {
-    float althold_lean_angle_max = acos(constrain_float(_throttle_in/AC_ATTITUDE_CONTROL_ALTHOLD_LEAN_ANGLE_THROTTLE_MAX, 0.0f, 1.0f));
-    _althold_lean_angle_max = _althold_lean_angle_max + (_dt/(_dt+AC_ATTITUDE_CONTROL_ALTHOLD_LEAN_ANGLE_TC_DEFAULT))*(_throttle_in-_althold_lean_angle_max);
+    float althold_lean_angle_max = acos(constrain_float(_throttle_in/AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX, 0.0f, 1.0f));
+    _althold_lean_angle_max = _althold_lean_angle_max + (_dt/(_dt+_angle_limit_tc))*(_throttle_in-_althold_lean_angle_max);
 }
 
 //

libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp

@@ -159,8 +159,8 @@ void AC_AttitudeControl_Multi::update_althold_lean_angle_max(float throttle_in)
         return;
     }
 
-    float althold_lean_angle_max = acos(constrain_float(_throttle_in/(AC_ATTITUDE_CONTROL_ALTHOLD_LEAN_ANGLE_THROTTLE_MAX * thr_max), 0.0f, 1.0f));
-    _althold_lean_angle_max = _althold_lean_angle_max + (_dt/(_dt+AC_ATTITUDE_CONTROL_ALTHOLD_LEAN_ANGLE_TC_DEFAULT))*(althold_lean_angle_max-_althold_lean_angle_max);
+    float althold_lean_angle_max = acos(constrain_float(_throttle_in/(AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX * thr_max), 0.0f, 1.0f));
+    _althold_lean_angle_max = _althold_lean_angle_max + (_dt/(_dt+_angle_limit_tc))*(althold_lean_angle_max-_althold_lean_angle_max);
 }
 
 void AC_AttitudeControl_Multi::set_throttle_out(float throttle_in, bool apply_angle_boost, float filter_cutoff)