diff --git a/src/modules/systemlib/battery.cpp b/src/modules/systemlib/battery.cpp index 4cb352158b..c8a8785470 100644 --- a/src/modules/systemlib/battery.cpp +++ b/src/modules/systemlib/battery.cpp @@ -50,7 +50,7 @@ Battery::Battery() : _param_v_load_drop(this, "V_LOAD_DROP"), _param_low_thr(this, "LOW_THR"), _param_crit_thr(this, "CRIT_THR"), - _voltage_filtered_v(0.0f), + _voltage_filtered_v(-1.0f), _throttle_filtered(0.0f), _discharged_mah(0.0f), _remaining(1.0f), @@ -84,7 +84,7 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre battery_status->timestamp = timestamp; filterVoltage(voltage_v); sumDischarged(timestamp, current_a); - estimateRemaining(voltage_v, throttle_normalized); + estimateRemaining(_voltage_filtered_v, throttle_normalized); determineWarning(); if (_voltage_filtered_v > 2.1f) { @@ -101,6 +101,10 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre void Battery::filterVoltage(float voltage_v) { + if (_voltage_filtered_v < 0) { + _voltage_filtered_v = voltage_v; + } + // TODO: inspect that filter performance const float filtered_next = _voltage_filtered_v * 0.999f + voltage_v * 0.001f;