AC_AttitudeControl: add TC for Alt_Hold angle limit

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -595,7 +595,6 @@ void AC_AttitudeControl::accel_limiting(bool enable_limits)
 void AC_AttitudeControl::set_throttle_out_unstabilized(float throttle_in, bool reset_attitude_control, float filter_cutoff)
 {
     _throttle_in = throttle_in;
-    _throttle_in_filt.apply(throttle_in, _dt);
     _motors.set_throttle_filter_cutoff(filter_cutoff);
     if (reset_attitude_control) {
         relax_bf_rate_controller();

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -40,7 +40,8 @@
 
 #define AC_ATTITUDE_CONTROL_RATE_BF_FF_DEFAULT          1       // body-frame rate feedforward enabled by default
 
-#define AC_ATTITUDE_CONTROL_ALTHOLD_LEANANGLE_FILT_HZ   1.0f    // filter (in hz) of throttle filter used to limit lean angle so that vehicle does not lose altitude
+#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_MIN_DEFAULT   0.1f    // minimum throttle mix
 #define AC_ATTITUDE_CONTROL_MID_DEFAULT   0.5f    // manual throttle mix
@@ -60,7 +61,6 @@ public:
         _dt(dt),
         _angle_boost(0),
         _att_ctrl_use_accel_limit(true),
-        _throttle_in_filt(AC_ATTITUDE_CONTROL_ALTHOLD_LEANANGLE_FILT_HZ),
         _throttle_rpy_mix_desired(AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT),
         _throttle_rpy_mix(AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT),
         _ahrs(ahrs),
@@ -198,6 +198,9 @@ public:
     // Enable or disable body-frame feed forward
     void accel_limiting(bool enable_or_disable);
 
+    // Update Alt_Hold angle maximum
+    virtual void update_althold_lean_angle_max(float throttle_in) = 0;
+
     // Set output throttle
     virtual void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) = 0;
 
@@ -341,7 +344,6 @@ protected:
     Vector3f            _ang_vel_target_rads;
 
     // throttle provided as input to attitude controller.  This does not include angle boost.
-    // Used only for logging.
     float               _throttle_in = 0.0f;
 
     // This represents the throttle increase applied for tilt compensation.
@@ -351,8 +353,8 @@ protected:
     // Specifies whether the attitude controller should use the acceleration limit
     bool                _att_ctrl_use_accel_limit;
 
-    // Filtered throttle input - used to limit lean angle when throttle is saturated
+    // Filtered Alt_Hold lean angle max - used to limit lean angle when throttle is saturated using Alt_Hold
-    LowPassFilterFloat  _throttle_in_filt;
+    float               _althold_lean_angle_max;
 
     float               _throttle_rpy_mix_desired;  // desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds
     float               _throttle_rpy_mix;          // mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1

libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp

@@ -235,11 +235,15 @@ void AC_AttitudeControl_Heli::rate_controller_run()
 // get lean angle max for pilot input that prioritises altitude hold over lean angle
 float AC_AttitudeControl_Heli::get_althold_lean_angle_max() const
 {
-    // calc maximum tilt angle based on throttle
-    float ret = acosf(constrain_float(_throttle_in_filt.get()/0.9f, 0.0f, 1.0f));
-
     // TEMP: convert to centi-degrees for public interface
-    return degrees(ret) * 100.0f;
+    return degrees(_althold_lean_angle_max) * 100.0f;
+}
+
+// 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);
 }
 
 //
@@ -407,7 +411,6 @@ float AC_AttitudeControl_Heli::rate_bf_to_motor_yaw(float rate_target_rads)
 void AC_AttitudeControl_Heli::set_throttle_out(float throttle_in, bool apply_angle_boost, float filter_cutoff)
 {
     _throttle_in = throttle_in;
-    _throttle_in_filt.apply(throttle_in, _dt);
     _motors.set_throttle_filter_cutoff(filter_cutoff);
     _motors.set_throttle(throttle_in);
     // Clear angle_boost for logging purposes

libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h

@@ -79,6 +79,9 @@ public:
     // NOTE: returns centi-degrees
     float get_althold_lean_angle_max() const;
 
+    // Update Alt_Hold angle maximum
+    void update_althold_lean_angle_max(float throttle_in) override;
+
 	// use_leaky_i - controls whether we use leaky i term for body-frame to motor output stage
 	void use_leaky_i(bool leaky_i) {  _flags_heli.leaky_i = leaky_i; }
     

libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp

@@ -149,22 +149,30 @@ AC_AttitudeControl_Multi::AC_AttitudeControl_Multi(AP_AHRS &ahrs, const AP_Vehic
 
 // get lean angle max for pilot input that prioritises altitude hold over lean angle
 float AC_AttitudeControl_Multi::get_althold_lean_angle_max() const
+{
+    return ToDeg(_althold_lean_angle_max) * 100.0f;
+}
+
+// Update Alt_Hold angle maximum
+void AC_AttitudeControl_Multi::update_althold_lean_angle_max(float throttle_in)
 {
     // calc maximum tilt angle based on throttle
     float thr_max = _motors_multi.get_throttle_thrust_max();
 
     // divide by zero check
     if (is_zero(thr_max)) {
-        return 0.0f;
+        _althold_lean_angle_max = 0.0f;
+        return;
     }
 
-    return ToDeg(acos(constrain_float(_throttle_in_filt.get()/(0.9f * thr_max), 0.0f, 1.0f))) * 100.0f;
+    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))*(_throttle_in-_althold_lean_angle_max);
 }
 
 void AC_AttitudeControl_Multi::set_throttle_out(float throttle_in, bool apply_angle_boost, float filter_cutoff)
 {
     _throttle_in = throttle_in;
-    _throttle_in_filt.apply(throttle_in, _dt);
+    update_althold_lean_angle_max(throttle_in);
     _motors.set_throttle_filter_cutoff(filter_cutoff);
     if (apply_angle_boost) {
         // Apply angle boost

libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h

@@ -55,6 +55,9 @@ public:
     // get lean angle max for pilot input that prioritizes altitude hold over lean angle
     float get_althold_lean_angle_max() const;
 
+    // Update Alt_Hold angle maximum
+    void update_althold_lean_angle_max(float throttle_in) override;
+
     // Set output throttle
     void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) override;