AC_AttitudeControl: tradheli - add support for throttle boost feature

libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp

@@ -544,7 +544,7 @@ void AC_AttitudeControl_Heli::set_throttle_out(float throttle_in, bool apply_ang
     } else {
         _transition_count = 0;
     }
-    float throttle_out = 0.0f;
+/*    float throttle_out = 0.0f;
     if (_transition_count > 0) {
         if ((_ahrs.roll_sensor >= -3000 && _ahrs.roll_sensor <= 3000) || _ahrs.roll_sensor >= 15000 || _ahrs.roll_sensor <= -15000) {
             throttle_out = (throttle_in - ((AP_MotorsHeli&)_motors).get_coll_mid()) / cosf(radians(_ahrs.roll_sensor * 0.01f)) + ((AP_MotorsHeli&)_motors).get_coll_mid();
@@ -557,11 +557,40 @@ void AC_AttitudeControl_Heli::set_throttle_out(float throttle_in, bool apply_ang
     } else {
         throttle_out = throttle_in;
     }
-
+*/
     _motors.set_throttle_filter_cutoff(filter_cutoff);
-    _motors.set_throttle(throttle_out);
+    if (apply_angle_boost) {
+        // Apply angle boost
+        throttle_in = get_throttle_boosted(throttle_in);
+    } else {
+        // Clear angle_boost for logging purposes
+        _angle_boost = 0.0f;
+    }
+    _motors.set_throttle(throttle_in);
     // Clear angle_boost for logging purposes
-    _angle_boost = 0.0f;
+//    _angle_boost = 0.0f;
+}
+
+// returns a throttle including compensation for roll/pitch angle
+// throttle value should be 0 ~ 1
+float AC_AttitudeControl_Multi::get_throttle_boosted(float throttle_in)
+{
+    if (!_angle_boost_enabled) {
+        _angle_boost = 0;
+        return throttle_in;
+    }
+    // inverted_factor is 1 for tilt angles below 60 degrees
+    // inverted_factor reduces from 1 to 0 for tilt angles between 60 and 90 degrees
+
+    float cos_tilt = _ahrs.cos_pitch() * _ahrs.cos_roll();
+    float inverted_factor = constrain_float(10.0f * cos_tilt, -1.0f, 1.0f);
+    float cos_tilt_target = fabsf(cosf(_thrust_angle));
+    float boost_factor = 1.0f / constrain_float(cos_tilt_target, 0.1f, 1.0f);
+
+    // angle boost and inverted factor applied about the zero thrust collective
+    float throttle_out = (throttle_in - ((AP_MotorsHeli&)_motors).get_coll_mid()) * inverted_factor * boost_factor + ((AP_MotorsHeli&)_motors).get_coll_mid();
+    _angle_boost = constrain_float(throttle_out - throttle_in, -1.0f, 1.0f);
+    return throttle_out;
 }
 
 // get_roll_trim - angle in centi-degrees to be added to roll angle for learn hover collective. Used by helicopter to counter tail rotor thrust in hover