diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 68c4d6935b..1121f70973 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -134,7 +134,7 @@ void Battery::updateBatteryStatus(const hrt_abstime ×tamp, 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 ×tamp, 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 ×tamp, 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; diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index bed43eaf4f..495a8ae4f3 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -196,7 +196,7 @@ protected: private: void sumDischarged(const hrt_abstime ×tamp, 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 _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};