Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 08:36:08 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

battery_status: factor in total current consumption in battery remaining estimation

Browse Source
This commit is contained in:
Sander Smeets
2021-03-15 17:32:45 +01:00
committed by GitHub
parent 82a2126f97
commit 517a1d0116
2 changed files with 21 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/battery/battery.cpp
+18 -15
View File
@@ -134,7 +134,7 @@ void Battery::updateBatteryStatus(const hrt_abstime &timestamp, float voltage_v,
_current_filter_a.update(current_a);
_throttle_filter.update(throttle_normalized);
sumDischarged(timestamp, current_a);
estimateRemaining(_voltage_filter_v.getState(), _current_filter_a.getState(), _throttle_filter.getState());
estimateStateOfCharge(_voltage_filter_v.getState(), _current_filter_a.getState(), _throttle_filter.getState());
computeScale();
if (_battery_initialized) {
@@ -150,7 +150,7 @@ void Battery::updateBatteryStatus(const hrt_abstime &timestamp, float voltage_v,
_battery_status.current_filtered_a = _current_filter_a.getState();
_battery_status.discharged_mah = _discharged_mah;
_battery_status.warning = _warning;
_battery_status.remaining = _remaining;
_battery_status.remaining = _state_of_charge;
_battery_status.connected = connected;
_battery_status.source = source;
_battery_status.priority = priority;
@@ -198,7 +198,7 @@ void Battery::sumDischarged(const hrt_abstime &timestamp, float current_a)
_last_timestamp = timestamp;
}
void Battery::estimateRemaining(const float voltage_v, const float current_a, const float throttle)
void Battery::estimateStateOfCharge(const float voltage_v, const float current_a, const float throttle)
{
// remaining battery capacity based on voltage
float cell_voltage = voltage_v / _params.n_cells;
@@ -212,27 +212,30 @@ void Battery::estimateRemaining(const float voltage_v, const float current_a, co
cell_voltage += throttle * _params.v_load_drop;
}
_remaining_voltage = math::gradual(cell_voltage, _params.v_empty, _params.v_charged, 0.f, 1.f);
_state_of_charge_volt_based = math::gradual(cell_voltage, _params.v_empty, _params.v_charged, 0.f, 1.f);
// choose which quantity we're using for final reporting
if (_params.capacity > 0.f) {
// if battery capacity is known, fuse voltage measurement with used capacity
if (!_battery_initialized) {
// initialization of the estimation state
_remaining = _remaining_voltage;
_state_of_charge = _state_of_charge_volt_based;
} else {
// The lower the voltage the more adjust the estimate with it to avoid deep discharge
const float weight_v = 3e-4f * (1 - _remaining_voltage);
_remaining = (1 - weight_v) * _remaining + weight_v * _remaining_voltage;
const float weight_v = 3e-4f * (1 - _state_of_charge_volt_based);
_state_of_charge = (1 - weight_v) * _state_of_charge + weight_v * _state_of_charge_volt_based;
// directly apply current capacity slope calculated using current
_remaining -= _discharged_mah_loop / _params.capacity;
_remaining = math::max(_remaining, 0.f);
_state_of_charge -= _discharged_mah_loop / _params.capacity;
_state_of_charge = math::max(_state_of_charge, 0.f);
const float state_of_charge_current_based = math::max(1.f - _discharged_mah / _params.capacity, 0.f);
_state_of_charge = math::min(state_of_charge_current_based, _state_of_charge);
}
} else {
// else use voltage
_remaining = _remaining_voltage;
_state_of_charge = _state_of_charge_volt_based;
}
}
@@ -240,13 +243,13 @@ void Battery::determineWarning(bool connected)
{
if (connected) {
// propagate warning state only if the state is higher, otherwise remain in current warning state
if (_remaining < _params.emergen_thr) {
if (_state_of_charge < _params.emergen_thr) {
_warning = battery_status_s::BATTERY_WARNING_EMERGENCY;
} else if (_remaining < _params.crit_thr) {
} else if (_state_of_charge < _params.crit_thr) {
_warning = battery_status_s::BATTERY_WARNING_CRITICAL;
} else if (_remaining < _params.low_thr) {
} else if (_state_of_charge < _params.low_thr) {
_warning = battery_status_s::BATTERY_WARNING_LOW;
} else {
@@ -260,7 +263,7 @@ void Battery::computeScale()
const float voltage_range = (_params.v_charged - _params.v_empty);
// reusing capacity calculation to get single cell voltage before drop
const float bat_v = _params.v_empty + (voltage_range * _remaining_voltage);
const float bat_v = _params.v_empty + (voltage_range * _state_of_charge_volt_based);
_scale = _params.v_charged / bat_v;
src/lib/battery/battery.h
+3 -3
View File
@@ -196,7 +196,7 @@ protected:
private:
void sumDischarged(const hrt_abstime &timestamp, float current_a);
void estimateRemaining(const float voltage_v, const float current_a, const float throttle);
void estimateStateOfCharge(const float voltage_v, const float current_a, const float throttle);
void determineWarning(bool connected);
void computeScale();
@@ -208,8 +208,8 @@ private:
AlphaFilter<float> _throttle_filter;
float _discharged_mah{0.f};
float _discharged_mah_loop{0.f};
float _remaining_voltage{-1.f}; ///< normalized battery charge level remaining based on voltage
float _remaining{-1.f}; ///< normalized battery charge level, selected based on config param
float _state_of_charge_volt_based{-1.f}; // [0,1]
float _state_of_charge{-1.f}; // [0,1]
float _scale{1.f};
uint8_t _warning{battery_status_s::BATTERY_WARNING_NONE};
hrt_abstime _last_timestamp{0};