diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 8f8690a2f2..e8c671e45a 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -118,6 +118,18 @@ void Battery::updateBatteryStatus(const hrt_abstime ×tamp) _current_filter_a.reset(_current_a); } + // Require minimum voltage toherwise override connected status + if (_voltage_filter_v.getState() < 2.1f) { + _connected = false; + } + + if (!_connected || (_last_unconnected_timestamp == 0)) { + _last_unconnected_timestamp = timestamp; + } + + // wait with initializing filters to avoid relying on a voltage sample from the rising edge + _battery_initialized = _connected && (timestamp > _last_unconnected_timestamp + 2_s); + sumDischarged(timestamp, _current_a); _state_of_charge_volt_based = calculateStateOfChargeVoltageBased(_voltage_filter_v.getState(), _current_filter_a.getState()); @@ -131,13 +143,6 @@ void Battery::updateBatteryStatus(const hrt_abstime ×tamp) if (_connected && _battery_initialized) { _warning = determineWarning(_state_of_charge); } - - if (_voltage_filter_v.getState() > 2.1f) { - _battery_initialized = true; - - } else { - _connected = false; - } } battery_status_s Battery::getBatteryStatus() diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index 9616a4e0cf..f7b5b20fd5 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -175,4 +175,5 @@ private: uint8_t _warning{battery_status_s::BATTERY_WARNING_NONE}; hrt_abstime _last_timestamp{0}; bool _armed{false}; + hrt_abstime _last_unconnected_timestamp{0}; };