diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 36f8590865..53b8199ee2 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -51,6 +51,10 @@ Battery::Battery(int index, ModuleParams *parent) : _warning(battery_status_s::BATTERY_WARNING_NONE), _last_timestamp(0) { + _voltage_filter_v.setParameters(.01f, 1.f); + _current_filter_a.setParameters(.01f, .5f); + _throttle_filter.setParameters(.01f, 1.f); + if (index > 9 || index < 1) { PX4_ERR("Battery index must be between 1 and 9 (inclusive). Received %d. Defaulting to 1.", index); } @@ -126,24 +130,31 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre { reset(); _battery_status.timestamp = timestamp; - filterVoltage(voltage_v); - filterThrottle(throttle_normalized); - filterCurrent(current_a); + + if (!_battery_initialized) { + _voltage_filter_v.reset(voltage_v); + _current_filter_a.reset(current_a); + _throttle_filter.reset(throttle_normalized); + } + + _voltage_filter_v.update(voltage_v); + _current_filter_a.update(current_a); + _throttle_filter.update(throttle_normalized); sumDischarged(timestamp, current_a); - estimateRemaining(_voltage_filtered_v, _current_filtered_a, _throttle_filtered); + estimateRemaining(_voltage_filter_v.getState(), _current_filter_a.getState(), _throttle_filter.getState()); computeScale(); if (_battery_initialized) { determineWarning(connected); } - if (_voltage_filtered_v > 2.1f) { + if (_voltage_filter_v.getState() > 2.1f) { _battery_initialized = true; _battery_status.voltage_v = voltage_v; - _battery_status.voltage_filtered_v = _voltage_filtered_v; + _battery_status.voltage_filtered_v = _voltage_filter_v.getState(); _battery_status.scale = _scale; _battery_status.current_a = current_a; - _battery_status.current_filtered_a = _current_filtered_a; + _battery_status.current_filtered_a = _current_filter_a.getState(); _battery_status.discharged_mah = _discharged_mah; _battery_status.warning = _warning; _battery_status.remaining = _remaining; @@ -175,49 +186,6 @@ Battery::publish() orb_publish_auto(ORB_ID(battery_status), &_orb_advert, &_battery_status, &_orb_instance, ORB_PRIO_DEFAULT); } -void -Battery::filterVoltage(float voltage_v) -{ - if (!_battery_initialized) { - _voltage_filtered_v = voltage_v; - } - - // TODO: inspect that filter performance - const float filtered_next = _voltage_filtered_v * 0.99f + voltage_v * 0.01f; - - if (PX4_ISFINITE(filtered_next)) { - _voltage_filtered_v = filtered_next; - } -} - -void -Battery::filterCurrent(float current_a) -{ - if (!_battery_initialized) { - _current_filtered_a = current_a; - } - - // ADC poll is at 100Hz, this will perform a low pass over approx 500ms - const float filtered_next = _current_filtered_a * 0.98f + current_a * 0.02f; - - if (PX4_ISFINITE(filtered_next)) { - _current_filtered_a = filtered_next; - } -} - -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) { @@ -241,7 +209,7 @@ Battery::sumDischarged(hrt_abstime timestamp, float current_a) } void -Battery::estimateRemaining(float voltage_v, float current_a, float throttle) +Battery::estimateRemaining(const float voltage_v, const float current_a, const float throttle) { // remaining battery capacity based on voltage float cell_voltage = voltage_v / _params.n_cells; diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index 5305f770ce..5b32bf7f1f 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -53,6 +53,7 @@ #include #include #include +#include /** * BatteryBase is a base class for any type of battery. @@ -212,18 +213,15 @@ protected: } 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); + void estimateRemaining(const float voltage_v, const float current_a, const float throttle); void determineWarning(bool connected); void computeScale(); bool _battery_initialized = false; - float _voltage_filtered_v = -1.f; - float _throttle_filtered = -1.f; - float _current_filtered_a = -1.f; + AlphaFilter _voltage_filter_v; + AlphaFilter _current_filter_a; + AlphaFilter _throttle_filter; float _discharged_mah = 0.f; float _discharged_mah_loop = 0.f; float _remaining_voltage = -1.f; ///< normalized battery charge level remaining based on voltage