Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: accept external battery resistance estimate

This commit is contained in:
Leonard Hall 2017-05-23 13:38:36 +09:30 committed by Randy Mackay
parent 7a2d8fd5fd
commit 9e86732edc
3 changed files with 10 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AP_Motors/AP_MotorsMulticopter.cpp
View File

@ -183,10 +183,6 @@ AP_MotorsMulticopter::AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz
AP_Motors(loop_rate, speed_hz),
_spool_mode(SHUT_DOWN),
_spin_up_ratio(0.0f),
_batt_voltage_resting(0.0f),
_batt_current_resting(0.0f),
_batt_resistance(0.0f),
_batt_timer(0),
_lift_max(1.0f),
_throttle_limit(1.0f),
_throttle_thrust_max(0.0f),
@ -212,9 +208,6 @@ void AP_MotorsMulticopter::output()
// update throttle filter
update_throttle_filter();
// update battery resistance
update_battery_resistance();
// calc filtered battery voltage and lift_max
update_lift_max_from_batt_voltage();
@ -328,45 +321,16 @@ void AP_MotorsMulticopter::update_lift_max_from_batt_voltage()
_batt_voltage_min = MAX(_batt_voltage_min, _batt_voltage_max * 0.6f);
// add current based voltage sag to battery voltage
float batt_voltage = _batt_voltage + (_batt_current-_batt_current_resting) * _batt_resistance;
batt_voltage = constrain_float(batt_voltage, _batt_voltage_min, _batt_voltage_max);
// contrain resting voltage estimate (resting voltage is actual voltage with sag removed based on current draw and resistance)
_batt_voltage_resting_estimate = constrain_float(_batt_voltage_resting_estimate, _batt_voltage_min, _batt_voltage_max);
// filter at 0.5 Hz
float batt_voltage_filt = _batt_voltage_filt.apply(batt_voltage/_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);
// calculate lift max
_lift_max = batt_voltage_filt*(1-_thrust_curve_expo) + _thrust_curve_expo*batt_voltage_filt*batt_voltage_filt;
}
// update_battery_resistance - calculate battery resistance when throttle is above hover_out
void AP_MotorsMulticopter::update_battery_resistance()
{
// if disarmed reset resting voltage and current
if (!_flags.armed) {
_batt_voltage_resting = _batt_voltage;
_batt_current_resting = _batt_current;
_batt_timer = 0;
} else if (_batt_voltage_resting > _batt_voltage && _batt_current_resting < _batt_current) {
// update battery resistance when throttle is over hover throttle
float batt_resistance = (_batt_voltage_resting-_batt_voltage)/(_batt_current-_batt_current_resting);
if ((_batt_timer < 400) && ((_batt_current_resting*2.0f) < _batt_current)) {
if (get_throttle() >= get_throttle_hover()) {
// filter reaches 90% in 1/4 the test time
_batt_resistance += 0.05f*(batt_resistance - _batt_resistance);
_batt_timer += 1;
} else {
// initialize battery resistance to prevent change in resting voltage estimate
_batt_resistance = batt_resistance;
}
}
// make sure battery resistance value doesn't result in the predicted battery voltage exceeding the resting voltage
if(batt_resistance < _batt_resistance){
_batt_resistance = batt_resistance;
}
}
}
float AP_MotorsMulticopter::get_compensation_gain() const
{
// avoid divide by zero

10
libraries/AP_Motors/AP_MotorsMulticopter.h
View File

@ -72,9 +72,6 @@ public:
// get_batt_voltage_filt - get battery voltage ratio - for logging purposes only
float get_batt_voltage_filt() const { return _batt_voltage_filt.get(); }
// get_batt_resistance - get battery resistance approximation - for logging purposes only
float get_batt_resistance() const { return _batt_resistance; }
// get throttle limit imposed by battery current limiting. This is a number from 0 ~ 1 where 0 means hover throttle, 1 means full throttle (i.e. not limited)
float get_throttle_limit() const { return _throttle_limit; }
@ -122,9 +119,6 @@ protected:
// update_lift_max_from_batt_voltage - used for voltage compensation
void update_lift_max_from_batt_voltage();
// update_battery_resistance - calculate battery resistance when throttle is above hover_out
void update_battery_resistance();
// return gain scheduling gain based on voltage and air density
float get_compensation_gain() const;
@ -185,11 +179,7 @@ protected:
float _spin_up_ratio; // throttle percentage (0 ~ 1) between zero and throttle_min
// battery voltage, current and air pressure compensation variables
float _batt_voltage_resting; // battery voltage reading at minimum throttle
LowPassFilterFloat _batt_voltage_filt; // filtered battery voltage expressed as a percentage (0 ~ 1.0) of batt_voltage_max
float _batt_current_resting; // battery's current when motors at minimum
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
float _throttle_thrust_max; // the maximum allowed throttle thrust 0.0 to 1.0 in the range throttle_min to throttle_max

7
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -107,10 +107,15 @@ public:
//
// set_voltage - set voltage to be used for output scaling
void set_voltage(float volts){ _batt_voltage = volts; }
void set_voltage_resting_estimate(float volts) { _batt_voltage_resting_estimate = volts; }
// set_current - set current to be used for output scaling
void set_current(float current){ _batt_current = current; }
// get and set battery resistance estimate
float get_batt_resistance() const { return _batt_resistance; }
void set_resistance(float resistance){ _batt_resistance = resistance; }
// set_density_ratio - sets air density as a proportion of sea level density
void set_air_density_ratio(float ratio) { _air_density_ratio = ratio; }
@ -207,7 +212,9 @@ protected:
// battery voltage, current and air pressure compensation variables
float _batt_voltage; // latest battery voltage reading
float _batt_voltage_resting_estimate; // estimated battery voltage with sag removed
float _batt_current; // latest battery current reading
float _batt_resistance; // latest battery resistance estimate in ohms
float _air_density_ratio; // air density / sea level density - decreases in altitude
// mapping to output channels