aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Motors: Allow specifying which battery is used for compensation

Browse Source
This commit is contained in:
Michael du Breuil 2018-03-21 14:14:33 -07:00 committed by Francisco Ferreira
parent 69cf009898
commit 050b8ebb32
2 changed files with 15 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_Motors/AP_MotorsMulticopter.cpp
View File

@ -176,6 +176,13 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
// 38 RESERVED for BAT_POW_MAX // 38 RESERVED for BAT_POW_MAX
// @Param: BAT_IDX
// @DisplayName: Battery compensation index
// @Description: Which battery monitor should be used for doing compensation
// @Values: 0:First battery, 1:Second battery
// @User: Advanced
AP_GROUPINFO("BAT_IDX", 39, AP_MotorsMulticopter, _batt_idx, 0),
AP_GROUPEND AP_GROUPEND
}; };
@ -270,22 +277,22 @@ float AP_MotorsMulticopter::get_current_limit_max_throttle()
if (_batt_current_max <= 0 || // return maximum if current limiting is disabled if (_batt_current_max <= 0 || // return maximum if current limiting is disabled
!_flags.armed || // remove throttle limit if disarmed !_flags.armed || // remove throttle limit if disarmed
!battery.has_current()) { // no current monitoring is available !battery.has_current(_batt_idx)) { // no current monitoring is available
_throttle_limit = 1.0f; _throttle_limit = 1.0f;
return 1.0f; return 1.0f;
} }
float _batt_resistance = battery.get_resistance(); float _batt_resistance = battery.get_resistance(_batt_idx);
if (is_zero(_batt_resistance)) { if (is_zero(_batt_resistance)) {
_throttle_limit = 1.0f; _throttle_limit = 1.0f;
return 1.0f; return 1.0f;
} }
float _batt_current = battery.current_amps(); float _batt_current = battery.current_amps(_batt_idx);
// calculate the maximum current to prevent voltage sag below _batt_voltage_min // calculate the maximum current to prevent voltage sag below _batt_voltage_min
float batt_current_max = MIN(_batt_current_max, _batt_current + (battery.voltage()-_batt_voltage_min)/_batt_resistance); float batt_current_max = MIN(_batt_current_max, _batt_current + (battery.voltage(_batt_idx)-_batt_voltage_min)/_batt_resistance);
float batt_current_ratio = _batt_current/batt_current_max; float batt_current_ratio = _batt_current/batt_current_max;
@ -323,7 +330,8 @@ void AP_MotorsMulticopter::update_lift_max_from_batt_voltage()
{ {
// sanity check battery_voltage_min is not too small // sanity check battery_voltage_min is not too small
// if disabled or misconfigured exit immediately // if disabled or misconfigured exit immediately
if((_batt_voltage_max <= 0) || (_batt_voltage_min >= _batt_voltage_max) || (AP::battery().voltage() < 0.25f*_batt_voltage_min)) { float _batt_voltage_resting_estimate = AP::battery().voltage_resting_estimate(_batt_idx);
if((_batt_voltage_max <= 0) || (_batt_voltage_min >= _batt_voltage_max) || (_batt_voltage_resting_estimate < 0.25f*_batt_voltage_min)) {
_batt_voltage_filt.reset(1.0f); _batt_voltage_filt.reset(1.0f);
_lift_max = 1.0f; _lift_max = 1.0f;
return; return;
@ -332,7 +340,7 @@ void AP_MotorsMulticopter::update_lift_max_from_batt_voltage()
_batt_voltage_min = MAX(_batt_voltage_min, _batt_voltage_max * 0.6f); _batt_voltage_min = MAX(_batt_voltage_min, _batt_voltage_max * 0.6f);
// contrain resting voltage estimate (resting voltage is actual voltage with sag removed based on current draw and resistance) // contrain resting voltage estimate (resting voltage is actual voltage with sag removed based on current draw and resistance)
float _batt_voltage_resting_estimate = constrain_float(AP::battery().voltage_resting_estimate(), _batt_voltage_min, _batt_voltage_max); _batt_voltage_resting_estimate = constrain_float(_batt_voltage_resting_estimate, _batt_voltage_min, _batt_voltage_max);
// filter at 0.5 Hz // filter at 0.5 Hz
float batt_voltage_filt = _batt_voltage_filt.apply(_batt_voltage_resting_estimate/_batt_voltage_max, 1.0f/_loop_rate); float batt_voltage_filt = _batt_voltage_filt.apply(_batt_voltage_resting_estimate/_batt_voltage_max, 1.0f/_loop_rate);

1
libraries/AP_Motors/AP_MotorsMulticopter.h
View File

@ -154,6 +154,7 @@ protected:
AP_Float _batt_voltage_min; // minimum 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 AP_Float _batt_current_max; // current over which maximum throttle is limited
AP_Float _batt_current_time_constant; // Time constant used to limit the maximum current AP_Float _batt_current_time_constant; // Time constant used to limit the maximum current
AP_Int8 _batt_idx; // battery index used for compensation
AP_Int16 _pwm_min; // minimum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's min pwm used) AP_Int16 _pwm_min; // minimum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's min pwm used)
AP_Int16 _pwm_max; // maximum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's max pwm used) AP_Int16 _pwm_max; // maximum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's max pwm used)
AP_Float _throttle_hover; // estimated throttle required to hover throttle in the range 0 ~ 1 AP_Float _throttle_hover; // estimated throttle required to hover throttle in the range 0 ~ 1