diff --git a/src/modules/systemlib/battery.cpp b/src/modules/systemlib/battery.cpp index 188fd7163a58c6b8fafa591a04192d578e8d0bdf..2302515707155488f0ad70df0febddd91fc15b13 100644 --- a/src/modules/systemlib/battery.cpp +++ b/src/modules/systemlib/battery.cpp @@ -71,9 +71,10 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre reset(battery_status); battery_status->timestamp = timestamp; filterVoltage(voltage_v); + filterThrottle(throttle_normalized); filterCurrent(current_a); sumDischarged(timestamp, current_a); - estimateRemaining(_voltage_filtered_v, _current_filtered_a, throttle_normalized, armed); + estimateRemaining(_voltage_filtered_v, _current_filtered_a, _throttle_filtered, armed); computeScale(); if (_battery_initialized) { @@ -126,6 +127,17 @@ Battery::filterCurrent(float current_a) } } +void Battery::filterThrottle(float throttle){ + if (!_battery_initialized) { + _throttle_filtered = throttle; + } + + const float filtered_next = _throttle_filtered * 0.99f + throttle * 0.01f; + + if (PX4_ISFINITE(filtered_next)){ + _throttle_filtered = filtered_next; + } +} void Battery::sumDischarged(hrt_abstime timestamp, float current_a) @@ -150,7 +162,7 @@ Battery::sumDischarged(hrt_abstime timestamp, float current_a) } void -Battery::estimateRemaining(float voltage_v, float current_a, float throttle_normalized, bool armed) +Battery::estimateRemaining(float voltage_v, float current_a, float throttle, bool armed) { // remaining battery capacity based on voltage float cell_voltage = voltage_v / _n_cells.get(); @@ -161,7 +173,7 @@ Battery::estimateRemaining(float voltage_v, float current_a, float throttle_norm } else { // assume linear relation between throttle and voltage drop - cell_voltage += throttle_normalized * _v_load_drop.get(); + cell_voltage += throttle * _v_load_drop.get(); } _remaining_voltage = math::gradual(cell_voltage, _v_empty.get(), _v_charged.get(), 0.f, 1.f); diff --git a/src/modules/systemlib/battery.h b/src/modules/systemlib/battery.h index fa9a4b1e5e728a19fda23b014fc9382a4ecb8a92..b007c43863d46ebff4980714461db7f4c30d9194 100644 --- a/src/modules/systemlib/battery.h +++ b/src/modules/systemlib/battery.h @@ -88,9 +88,10 @@ public: private: void filterVoltage(float voltage_v); + void filterThrottle(float throttle); void filterCurrent(float current_a); void sumDischarged(hrt_abstime timestamp, float current_a); - void estimateRemaining(float voltage_v, float current_a, float throttle_normalized, bool armed); + void estimateRemaining(float voltage_v, float current_a, float throttle, bool armed); void determineWarning(bool connected); void computeScale(); @@ -108,6 +109,7 @@ private: bool _battery_initialized = false; float _voltage_filtered_v = -1.f; + float _throttle_filtered = -1.f; float _current_filtered_a = -1.f; float _discharged_mah = 0.f; float _discharged_mah_loop = 0.f;