From 529c6fed3ac4fb13b18eeeea8cbac323be36d651 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 23 Feb 2015 18:09:08 +0900 Subject: [PATCH] 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 | 24 ++----------- libraries/AP_Motors/AP_Motors_Class.cpp | 46 ++++++++++++++++++++++--- libraries/AP_Motors/AP_Motors_Class.h | 5 ++- 3 files changed, 47 insertions(+), 28 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 850c7c9b2f..5bcc4a9aaf 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/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); diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index 3987d19edf..53ffc47c44 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/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) diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 4589f2ffbd..323a80e981 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/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;