AC_AttitudeControl: rework angle_boost

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -698,7 +698,7 @@ void AC_AttitudeControl::set_throttle_out(float throttle_out, bool apply_angle_b
 {
     _motors.set_stabilizing(true);
     if (apply_angle_boost) {
-        _motors.set_throttle(get_angle_boost(throttle_out));
+        _motors.set_throttle(get_boosted_throttle(throttle_out));
     }else{
         _motors.set_throttle(throttle_out);
         // clear angle_boost for logging purposes
@@ -718,25 +718,20 @@ void AC_AttitudeControl::set_throttle_out_unstabilized(float throttle_in, bool r
     _angle_boost = 0;
 }
 
-// get_angle_boost - returns a throttle including compensation for roll/pitch angle
+// returns a throttle including compensation for roll/pitch angle
 // throttle value should be 0 ~ 1000
-float AC_AttitudeControl::get_angle_boost(float throttle_pwm)
+float AC_AttitudeControl::get_boosted_throttle(float throttle_in)
 {
-    float temp = _ahrs.cos_pitch() * _ahrs.cos_roll();
-    float throttle_out;
-
-    temp = constrain_float(temp, 0.5f, 1.0f);
-
-    // reduce throttle if we go inverted
-    temp = constrain_float(9000-max(labs(_ahrs.roll_sensor),labs(_ahrs.pitch_sensor)), 0, 3000) / (3000 * temp);
-
-    // apply scale and constrain throttle
-    // To-Do: move throttle_min and throttle_max into the AP_Vehicles class?
-    throttle_out = constrain_float((float)(throttle_pwm-_motors.throttle_min()) * temp + _motors.throttle_min(), _motors.throttle_min(), 1000);
-
-    // record angle boost for logging
-    _angle_boost = throttle_out - throttle_pwm;
-
+    // inverted_factor is 1 for tilt angles below 60 degrees
+    // reduces as a function of angle beyond 60 degrees
+    // becomes zero at 90 degrees
+    float min_throttle = _motors.throttle_min();
+    float cos_tilt = _ahrs.cos_pitch() * _ahrs.cos_roll();
+    float inverted_factor = constrain_float(2.0f*cos_tilt, 0.0f, 1.0f);
+    float boost_factor = 1.0f/constrain_float(cos_tilt, 0.5f, 1.0f);
+
+    float throttle_out = (throttle_in-min_throttle)*inverted_factor*boost_factor + min_throttle;
+    _angle_boost = throttle_out - throttle_in;
     return throttle_out;
 }
 

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -238,8 +238,8 @@ protected:
     // throttle methods
     //
 
-    // get_angle_boost - calculate total body frame throttle required to produce the given earth frame throttle
-    virtual float get_angle_boost(float throttle_pwm);
+    // calculate total body frame throttle required to produce the given earth frame throttle
+    virtual float get_boosted_throttle(float throttle_in);
 
     // references to external libraries
     const AP_AHRS&      _ahrs;

libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp

@@ -361,13 +361,13 @@ float AC_AttitudeControl_Heli::rate_bf_to_motor_yaw(float rate_target_cds)
 // throttle functions
 //
 
-// get_angle_boost - returns a throttle including compensation for roll/pitch angle
+// returns a throttle including compensation for roll/pitch angle
 // throttle value should be 0 ~ 1000
-int16_t AC_AttitudeControl_Heli::get_angle_boost(int16_t throttle_pwm)
+float AC_AttitudeControl_Heli::get_boosted_throttle(float throttle_in)
 {
     // no angle boost for trad helis
     _angle_boost = 0;
-    return throttle_pwm;
+    return throttle_in;
 }
 
 // update_feedforward_filter_rate - Sets LPF cutoff frequency

libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h

@@ -73,8 +73,8 @@ private:
     // throttle methods
     //
 
-    // get_angle_boost - calculate total body frame throttle required to produce the given earth frame throttle
-    virtual int16_t get_angle_boost(int16_t throttle_pwm);
+    // calculate total body frame throttle required to produce the given earth frame throttle
+    float get_boosted_throttle(float throttle_in);
     
     
     // LPF filters to act on Rate Feedforward terms to linearize output.