From 919b6ae239647f4df6aa23f69b21d94d66811df8 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 27 Oct 2017 16:49:35 +0200 Subject: [PATCH] Battery added proper initialization for the capacity voltage fusion and doubled the time constant --- src/modules/systemlib/battery.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/modules/systemlib/battery.cpp b/src/modules/systemlib/battery.cpp index 060eb659de..67d4db583e 100644 --- a/src/modules/systemlib/battery.cpp +++ b/src/modules/systemlib/battery.cpp @@ -59,7 +59,7 @@ Battery::Battery() : _discharged_mah_loop(0.f), _remaining_voltage(1.f), _remaining_capacity(1.f), - _remaining(1.f), + _remaining(2.f), _scale(1.f), _warning(battery_status_s::BATTERY_WARNING_NONE), _last_timestamp(0) @@ -191,9 +191,15 @@ Battery::estimateRemaining(float voltage_v, float current_a, float throttle_norm _remaining_capacity = math::max(1.f - _discharged_mah / _capacity.get(), 0.f); // if battery capacity is known, fuse voltage measurement with used capacity - _remaining = 0.999f * _remaining + 0.001f * _remaining_voltage; - _remaining -= _discharged_mah_loop / _capacity.get(); - _remaining = math::max(_remaining, 0.f); + if (_remaining > 1.f) { + // initialization of the state + _remaining = _remaining_voltage; + + } else { + _remaining = 0.9995f * _remaining + 0.0005f * _remaining_voltage; + _remaining -= _discharged_mah_loop / _capacity.get(); + _remaining = math::max(_remaining, 0.f); + } } else { // else use voltage