Motors: move over current throttle limiting to parent

Moving from MotorsMatrix to parent Motors class allows this to be used
from other frame types

libraries/AP_Motors/AP_MotorsMatrix.cpp

@@ -190,26 +190,6 @@ void AP_MotorsMatrix::output_armed()
             limit.throttle_lower = true;
         }
 
-        // limit throttle if over current
-        if(_batt_current_max > 0){
-            if(_batt_current > _batt_current_max*1.25f){
-                // Fast drop for extreme over current (1 second)
-                _throttle_limit -= 1.0f/_speed_hz;
-            }else if(_batt_current > _batt_current_max){
-                // Slow drop for extreme over current (2 second)
-                _throttle_limit -= 0.5f/_speed_hz;
-            }else{
-                // Increase throttle limit (2 second)
-                _throttle_limit += 0.5f/_speed_hz;
-            }
-        } else {
-            _throttle_limit = 1.0f;
-        }
-        // throttle limit drops to 20% between hover and full throttle
-        _throttle_limit = constrain_float(_throttle_limit, 0.2f, 1.0f);
-
-        int16_t throttle_radio_out = min(_rc_throttle.radio_out, (_hover_out + (out_max_pwm-_hover_out)*_throttle_limit));
-
         // calculate battery resistance
         int16_t hov_out_pwm = get_hover_throttle_as_pwm();
         if (_batt_timer < 400 && _rc_throttle.radio_out >= hov_out_pwm && ((_batt_current_resting*2.0f) < _batt_current)) {
@@ -246,7 +226,7 @@ void AP_MotorsMatrix::output_armed()
         //      We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favour reducing throttle *because* it provides better yaw control)
         //      We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low.  We favour reducing throttle instead of better yaw control because the pilot has commanded it
         int16_t motor_mid = (rpy_low+rpy_high)/2;
-        out_best_thr_pwm = min(out_mid_pwm - motor_mid, max(throttle_radio_out, throttle_radio_out*max(0,1.0f-_throttle_low_comp)+hov_out_pwm*_throttle_low_comp));
+        out_best_thr_pwm = min(out_mid_pwm - motor_mid, max(_rc_throttle.radio_out, _rc_throttle.radio_out*max(0,1.0f-_throttle_low_comp)+hov_out_pwm*_throttle_low_comp));
 
         // calculate amount of yaw we can fit into the throttle range
         // this is always equal to or less than the requested yaw from the pilot or rate controller
@@ -290,7 +270,7 @@ void AP_MotorsMatrix::output_armed()
         }
 
         // check everything fits
-        thr_adj = throttle_radio_out - out_best_thr_pwm;
+        thr_adj = _rc_throttle.radio_out - out_best_thr_pwm;
 
         // calculate upper and lower limits of thr_adj
         int16_t thr_adj_max = max(out_max_pwm-(out_best_thr_pwm+rpy_high),0);

libraries/AP_Motors/AP_Motors_Class.cpp

@@ -204,11 +204,8 @@ void AP_Motors::update_max_throttle()
         }
     }
 
-    // return max throttle if we're not slow_starting
-    if (!_flags.slow_start) {
-        return;
-    }
-
+    // implement slow start
+    if (_flags.slow_start) {
     // increase slow start throttle
     _max_throttle += AP_MOTOR_SLOW_START_INCREMENT;
 
@@ -217,6 +214,45 @@ void AP_Motors::update_max_throttle()
         _max_throttle = AP_MOTORS_DEFAULT_MAX_THROTTLE;
         _flags.slow_start = false;
     }
+        return;
+    }
+
+    // current limit throttle
+    current_limit_max_throttle();
+}
+
+// current_limit_max_throttle - limits maximum throttle based on current
+void AP_Motors::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;
+    }
+
+    // remove throttle limit if throttle is at zero or disarmed
+    if(_rc_throttle.servo_out <= 0 || !_flags.armed) {
+        _throttle_limit = 1.0f;
+    }
+
+    // limit throttle if over current
+    if (_batt_current > _batt_current_max*1.25f) {
+        // Fast drop for extreme over current (1 second)
+        _throttle_limit -= 1.0f/_loop_rate;
+    } else if(_batt_current > _batt_current_max) {
+        // Slow drop for extreme over current (2 second)
+        _throttle_limit -= 0.5f/_loop_rate;
+    } else {
+        // Increase throttle limit (2 second)
+        _throttle_limit += 0.5f/_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
+    _max_throttle = _hover_out + ((1000-_hover_out)*_throttle_limit);
 }
 
 // apply_thrust_curve_and_volt_scaling - returns throttle curve adjusted pwm value (i.e. 1000 ~ 2000)

libraries/AP_Motors/AP_Motors_Class.h

@@ -176,9 +176,12 @@ protected:
     virtual void        output_armed() {};
     virtual void        output_disarmed() {};
 
-    // update_max_throttle - updates the limits on _max_throttle if necessary taking into account slow_start_throttle flag
+    // update_max_throttle - updates the limits on _max_throttle for slow_start and current limiting flag
     void                update_max_throttle();
 
+    // current_limit_max_throttle - current limit maximum throttle (called from update_max_throttle)
+    void                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(int16_t pwm_out, int16_t pwm_min, int16_t pwm_max) const;