From b1d20970538e8ee5f992293bd11610ae6cf1c8b6 Mon Sep 17 00:00:00 2001 From: nanthony21 Date: Sat, 10 Mar 2018 19:09:56 -0600 Subject: [PATCH 1/2] Use filtered throttle for battery estimation instead of raw. --- src/modules/systemlib/battery.cpp | 18 +++++++++++++++--- src/modules/systemlib/battery.h | 4 +++- 2 files changed, 18 insertions(+), 4 deletions(-) diff --git a/src/modules/systemlib/battery.cpp b/src/modules/systemlib/battery.cpp index 5b45c9044190..9ad689dcbb17 100644 --- a/src/modules/systemlib/battery.cpp +++ b/src/modules/systemlib/battery.cpp @@ -86,9 +86,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) { @@ -141,6 +142,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) @@ -165,7 +177,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(); @@ -176,7 +188,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 bb0a754e9764..877d1c87c4dd 100644 --- a/src/modules/systemlib/battery.h +++ b/src/modules/systemlib/battery.h @@ -97,9 +97,10 @@ class Battery : public control::SuperBlock 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(); @@ -115,6 +116,7 @@ class Battery : public control::SuperBlock 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; From 940b45376e1d40679520617e97738a267bf6fd8a Mon Sep 17 00:00:00 2001 From: nanthony21 Date: Sun, 11 Mar 2018 00:15:50 -0600 Subject: [PATCH 2/2] Fix formatting --- src/modules/systemlib/battery.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/battery.cpp b/src/modules/systemlib/battery.cpp index 9ad689dcbb17..a758cc5076a2 100644 --- a/src/modules/systemlib/battery.cpp +++ b/src/modules/systemlib/battery.cpp @@ -142,14 +142,15 @@ Battery::filterCurrent(float current_a) } } -void Battery::filterThrottle(float throttle){ +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)){ + if (PX4_ISFINITE(filtered_next)) { _throttle_filtered = filtered_next; } }