diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 0a52e3f6d6..a9274a7187 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -202,6 +202,26 @@ 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 if (_batt_timer < 400 && _rc_throttle.radio_out >= _hover_out && ((_batt_current_resting*2.0f) < _batt_current)) { // filter reaches 90% in 1/4 the test time @@ -237,7 +257,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(_rc_throttle.radio_out, _rc_throttle.radio_out*max(0,1.0f-_throttle_low_comp)+_hover_out*_throttle_low_comp)); + out_best_thr_pwm = min(out_mid_pwm - motor_mid, max(throttle_radio_out, throttle_radio_out*max(0,1.0f-_throttle_low_comp)+_hover_out*_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 @@ -281,7 +301,7 @@ void AP_MotorsMatrix::output_armed() } // check everything fits - thr_adj = _rc_throttle.radio_out - out_best_thr_pwm; + thr_adj = throttle_radio_out - out_best_thr_pwm; // calc upper and lower limits of thr_adj int16_t thr_adj_max = max(out_max_pwm-(out_best_thr_pwm+rpy_high),0); diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index c1ac57761c..23065efe14 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -114,6 +114,14 @@ const AP_Param::GroupInfo AP_Motors::var_info[] PROGMEM = { // @User: Advanced AP_GROUPINFO("THST_BAT_MIN", 11, AP_Motors, _batt_voltage_min, AP_MOTORS_THST_BAT_MIN_DEFAULT), + // @Param: CURR_MAX + // @DisplayName: Motor Current Max + // @Description: Maximum current over which maximum throttle is limited (0 = Disabled) + // @Range: 0 200 + // @Units: Amps + // @User: Advanced + AP_GROUPINFO("CURR_MAX", 12, AP_Motors, _batt_current_max, AP_MOTORS_CURR_MAX_DEFAULT), + AP_GROUPEND }; @@ -135,7 +143,8 @@ AP_Motors::AP_Motors( RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_ _batt_current_resting(0.0f), _batt_resistance(0.0f), _batt_timer(0), - _lift_max(1.0f) + _lift_max(1.0f), + _throttle_limit(1.0f) { AP_Param::setup_object_defaults(this, var_info); diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index d965865962..85455b1950 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -61,6 +61,7 @@ #define AP_MOTORS_THST_MAX_DEFAULT 0.95f // throttle which produces the maximum thrust. (i.e. 0 ~ 1 ) of the full throttle range #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 // bit mask for recording which limits we have reached when outputting to motors #define AP_MOTOR_NO_LIMITS_REACHED 0x00 @@ -203,6 +204,7 @@ protected: AP_Float _thrust_curve_max; // throttle which produces the maximum thrust. (i.e. 0 ~ 1 ) of the full throttle range AP_Float _batt_voltage_max; // maximum voltage used to scale lift AP_Float _batt_voltage_min; // minimum voltage used to scale lift + AP_Float _batt_current_max; // current over which maximum throttle is limited // internal variables RC_Channel& _rc_roll; // roll input in from users is held in servo_out @@ -224,5 +226,6 @@ protected: float _batt_resistance; // battery's resistance calculated by comparing resting voltage vs in flight voltage int16_t _batt_timer; // timer used in battery resistance calcs float _lift_max; // maximum lift ratio from battery voltage + float _throttle_limit; // ratio of throttle limit between hover and maximum }; #endif // __AP_MOTORS_CLASS_H__