From 13c3eae6d00a94c56c3688e9045196d0fd4b4fd7 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 31 Mar 2022 16:38:53 +0200 Subject: [PATCH] battery: use mechanism to keep an up to date armed state --- src/lib/battery/battery.cpp | 12 ++++++++---- src/lib/battery/battery.h | 1 + 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index c3684f3cae..c3e0d3d4f7 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -56,7 +56,6 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us, _voltage_filter_v.setParameters(expected_filter_dt, 1.f); _current_filter_a.setParameters(expected_filter_dt, .5f); _current_average_filter_a.setParameters(expected_filter_dt, 50.f); - _throttle_filter.setParameters(expected_filter_dt, 1.f); if (index > 9 || index < 1) { @@ -275,14 +274,19 @@ float Battery::computeRemainingTime(float current_a) // Remaining time estimation only possible with capacity if (_params.capacity > 0.f) { - vehicle_status_s vehicle_status{}; - _vehicle_status_sub.copy(&vehicle_status); + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + _armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + } + } if (!PX4_ISFINITE(_current_average_filter_a.getState()) || _current_average_filter_a.getState() < FLT_EPSILON) { _current_average_filter_a.reset(_params.bat_avrg_current); } - if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED && PX4_ISFINITE(current_a)) { + if (_armed && PX4_ISFINITE(current_a)) { // only update with positive numbers _current_average_filter_a.update(fmaxf(current_a, 0.f)); } diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index c521bdf675..24470b6a0d 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -170,4 +170,5 @@ private: float _scale{1.f}; uint8_t _warning{battery_status_s::BATTERY_WARNING_NONE}; hrt_abstime _last_timestamp{0}; + bool _armed{false}; };