AC_AttControl: limit lean angle from throttle

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -702,6 +702,7 @@ void AC_AttitudeControl::accel_limiting(bool enable_limits)
  // provide 0 to cut motors
 void AC_AttitudeControl::set_throttle_out(float throttle_out, bool apply_angle_boost, float filter_cutoff)
 {
+    _throttle_in_filt.apply(throttle_out, _dt);
     _motors.set_stabilizing(true);
     _motors.set_throttle_filter_cutoff(filter_cutoff);
     if (apply_angle_boost) {
@@ -716,6 +717,7 @@ void AC_AttitudeControl::set_throttle_out(float throttle_out, bool apply_angle_b
 // outputs a throttle to all motors evenly with no attitude stabilization
 void AC_AttitudeControl::set_throttle_out_unstabilized(float throttle_in, bool reset_attitude_control, float filter_cutoff)
 {
+    _throttle_in_filt.apply(throttle_in, _dt);
     if (reset_attitude_control) {
         relax_bf_rate_controller();
         set_yaw_target_to_current_heading();

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -41,6 +41,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
+
 class AC_AttitudeControl {
 public:
 	AC_AttitudeControl( AP_AHRS &ahrs,
@@ -60,7 +62,8 @@ public:
         _pid_rate_yaw(pid_rate_yaw),
         _dt(AC_ATTITUDE_100HZ_DT),
         _angle_boost(0),
-        _acro_angle_switch(0)
+        _acro_angle_switch(0),
+	    _throttle_in_filt(AC_ATTITUDE_CONTROL_ALTHOLD_LEANANGLE_FILT_HZ)
 		{
 			AP_Param::setup_object_defaults(this, var_info);
 
@@ -211,6 +214,9 @@ public:
      // angle_boost - accessor for angle boost so it can be logged
      int16_t angle_boost() const { return _angle_boost; }
 
+     // get lean angle max for pilot input that prioritises altitude hold over lean angle
+     virtual float get_althold_lean_angle_max() const = 0;
+
     //
     // helper functions
     //
@@ -293,6 +299,9 @@ protected:
     Vector3f            _rate_bf_desired;       // body-frame feed forward rates
     int16_t             _angle_boost;           // used only for logging
     int16_t             _acro_angle_switch;           // used only for logging
+
+    // throttle based angle limits
+    LowPassFilterFloat  _throttle_in_filt;      // throttle input from pilot or alt hold controller
 };
 
 #define AC_ATTITUDE_CONTROL_LOG_FORMAT(msg) { msg, sizeof(AC_AttitudeControl::log_Attitude),	\

libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp

@@ -94,6 +94,13 @@ 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
+    return ToDeg(acos(constrain_float(_throttle_in_filt.get()/900.0f, 0.0f, 1000.0f) / 1000.0f)) * 100.0f;
+}
+
 //
 // private methods
 //

libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h

@@ -43,6 +43,9 @@ public:
 	// should be called at 100hz or more
 	virtual void rate_controller_run();
 
+    // get lean angle max for pilot input that prioritises altitude hold over lean angle
+    float get_althold_lean_angle_max() const;
+
 	// 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

@@ -4,6 +4,14 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Math/AP_Math.h>
 
+// get lean angle max for pilot input that prioritises altitude hold over lean angle
+float AC_AttitudeControl_Multi::get_althold_lean_angle_max() const
+{
+    // calc maximum tilt angle based on throttle
+    float thr_max = _motors_multi.throttle_max();
+    return ToDeg(acos(constrain_float(_throttle_in_filt.get()/(0.9f * thr_max / 1000.0f), 0.0f, 1000.0f) / 1000.0f)) * 100.0f;
+}
+
 // returns a throttle including compensation for roll/pitch angle
 // throttle value should be 0 ~ 1000
 float AC_AttitudeControl_Multi::get_boosted_throttle(float throttle_in)

libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h

@@ -24,6 +24,9 @@ public:
 	// empty destructor to suppress compiler warning
 	virtual ~AC_AttitudeControl_Multi() {}
 
+    // get lean angle max for pilot input that prioritises altitude hold over lean angle
+    float get_althold_lean_angle_max() const;
+
     // calculate total body frame throttle required to produce the given earth frame throttle
     float get_boosted_throttle(float throttle_in);