zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Motors: accept external battery resistance estimate

master
Leonard Hall 8 years ago committed by Randy Mackay
parent
7a2d8fd5fd
commit
9e86732edc
3 changed files with 10 additions and 49 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 42
      libraries/AP_Motors/AP_MotorsMulticopter.cpp
  2. 10
      libraries/AP_Motors/AP_MotorsMulticopter.h
  3. 7
      libraries/AP_Motors/AP_Motors_Class.h

42
libraries/AP_Motors/AP_MotorsMulticopter.cpp
Unescape View File

@ -183,10 +183,6 @@ AP_MotorsMulticopter::AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz @@ -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() @@ -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() @@ -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
Unescape View File

@ -72,9 +72,6 @@ public: @@ -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: @@ -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: @@ -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
Unescape View File

@ -107,10 +107,15 @@ public: @@ -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: @@ -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

Write Preview
Loading…
Cancel
Save