AP_MotorsMulticopter: add get_current_limit_max_throttle

returns the current limited maximum throttle as a number between 0 ~ 1 in the range throttle_min to throttle_max

libraries/AP_Motors/AP_MotorsMulticopter.cpp

@@ -216,7 +216,7 @@ void AP_MotorsMulticopter::current_limit_max_throttle()
     }
 
     // remove throttle limit if throttle is at zero or disarmed
-    if(_throttle_control_input <= 0 || !_flags.armed) {
+    if(_throttle_in <= 0.0f || !_flags.armed) {
         _throttle_limit = 1.0f;
     }
 
@@ -239,6 +239,36 @@ void AP_MotorsMulticopter::current_limit_max_throttle()
     _max_throttle = _hover_out + ((1000-_hover_out)*_throttle_limit);
 }
 
+// return current_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max
+//todo: replace this with a variable P term
+float AP_MotorsMulticopter::get_current_limit_max_throttle()
+{
+    // return maximum if current limiting is disabled
+    if (_batt_current_max <= 0) {
+        _throttle_limit = 1.0f;
+        _max_throttle = AP_MOTORS_DEFAULT_MAX_THROTTLE;
+        return 1.0f;
+    }
+
+    // remove throttle limit if throttle is at zero or disarmed
+    if(_throttle_in <= 0.0f || !_flags.armed) {
+        _throttle_limit = 1.0f;
+        _max_throttle = AP_MOTORS_DEFAULT_MAX_THROTTLE;
+        return 1.0f;
+    }
+
+    float batt_current_ratio = _batt_current/_batt_current_max;
+
+    _throttle_limit += AP_MOTORS_CURRENT_LIMIT_P*(1.0f - batt_current_ratio)/_loop_rate;
+
+    // throttle limit drops to 20% between hover and full throttle
+    _throttle_limit = constrain_float(_throttle_limit, 0.2f, 1.0f);
+
+    // limit max throttle
+    float throttle_thrust_hover = get_hover_throttle_as_high_end_pct();
+    return throttle_thrust_hover + ((1.0-throttle_thrust_hover)*_throttle_limit);
+}
+
 // apply_thrust_curve_and_volt_scaling - returns throttle curve adjusted pwm value (i.e. 1000 ~ 2000)
 int16_t AP_MotorsMulticopter::apply_thrust_curve_and_volt_scaling_pwm(int16_t pwm_out, int16_t pwm_min, int16_t pwm_max) const
 {

libraries/AP_Motors/AP_MotorsMulticopter.h

@@ -22,6 +22,7 @@
 #define AP_MOTORS_THST_BAT_MAX_DEFAULT  0.0f
 #define AP_MOTORS_THST_BAT_MIN_DEFAULT  0.0f
 #define AP_MOTORS_CURR_MAX_DEFAULT      0.0f    // current limiting max default
+#define AP_MOTORS_CURRENT_LIMIT_P       0.2f    // replace with parameter - Sets the current limit P term
 #define AP_MOTORS_BATT_VOLT_FILT_HZ     0.5f    // battery voltage filtered at 0.5hz
 #define AP_MOTORS_THR_MIX_MIN_DEFAULT   0.1f    // minimum throttle mix
 #define AP_MOTORS_THR_MIX_MID_DEFAULT   0.5f    // manual throttle mix
@@ -106,6 +107,9 @@ protected:
     // current_limit_max_throttle - current limit maximum throttle (called from update_max_throttle)
     void                current_limit_max_throttle();
 
+    // return current_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max
+    float               get_current_limit_max_throttle();
+
     // apply_thrust_curve_and_volt_scaling - thrust curve and voltage adjusted pwm value (i.e. 1000 ~ 2000)
     int16_t             apply_thrust_curve_and_volt_scaling_pwm(int16_t pwm_out, int16_t pwm_min, int16_t pwm_max) const;
     float               apply_thrust_curve_and_volt_scaling(float thrust) const;