Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Motors: over current throttle limiting

Browse Source
This commit is contained in:
Leonard Hall 2015-02-23 15:36:23 +09:00 committed by Randy Mackay
parent 8eedb2c040
commit 1d0ee68116
3 changed files with 35 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -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);

11
libraries/AP_Motors/AP_Motors_Class.cpp
View File

@ -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);

3
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -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__