From 42afefff783054daec1ed7245114087deffd36a5 Mon Sep 17 00:00:00 2001 From: Saibernard <112599512+Saibernard@users.noreply.github.com> Date: Sat, 29 Aug 2026 15:49:27 -0400 Subject: [PATCH] fix(battery_status): keep analog filter state across parameter updates (#28452) * fix(battery_status): seed the analog filters from the first sample Nothing seeded the voltage and current filters, so after boot the filtered values converged from zero over the filter time constant, several seconds of falsely low battery readings. Seed each filter with its first sample. Assisted-by: Claude:claude-fable-5 Signed-off-by: Saibernard Yogendran * fix(battery_status): reseed the filters after they are enabled again A filter that was disabled and later enabled kept its seeded flag, so it resumed from the stale state it held before being disabled. Clear the flag when a filter is disabled so enabling it again restarts from the live measurement. Assisted-by: Claude:claude-fable-5 Signed-off-by: Saibernard Yogendran * fix(battery_status): keep the analog filter state across parameter updates updateParams replaced the voltage and current filters with freshly constructed ones, which zeroes their state. It runs on any parameter change anywhere in the system, so with filtering enabled every change restarted the filtered voltage and current from zero. The next published voltage then sits below the 2.1 V battery recognition threshold, which reports the battery as disconnected, and while armed the battery warning only ever escalates, so a single in flight param set could latch a critical battery failsafe and command RTL or land per COM_LOW_BAT_ACT. Recovery of the filtered value takes several time constants, tens of seconds at the BAT_V_FILT maximum of 5 s. Carry the previous state across the reconstruction once the filter has been seeded. Assisted-by: Claude:claude-fable-5 Signed-off-by: Saibernard Yogendran --------- Signed-off-by: Saibernard Yogendran (cherry picked from commit 3b1517ccd869efd3f560b785c252b30cf759ff0e) --- src/modules/battery_status/analog_battery.cpp | 28 +++++++++++++++++++ src/modules/battery_status/analog_battery.h | 2 ++ 2 files changed, 30 insertions(+) diff --git a/src/modules/battery_status/analog_battery.cpp b/src/modules/battery_status/analog_battery.cpp index 68a9a4ad1380..fc691cd54f93 100644 --- a/src/modules/battery_status/analog_battery.cpp +++ b/src/modules/battery_status/analog_battery.cpp @@ -91,10 +91,20 @@ AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, _last_timestamp = timestamp; if (_analog_params.v_filt > FLT_EPSILON) { + if (!_voltage_filter_seeded) { + _voltage_filter.reset(fmaxf(voltage_v, 0.f)); + _voltage_filter_seeded = true; + } + voltage_v = _voltage_filter.update(fmaxf(voltage_v, 0.f), dt); } if (_analog_params.i_filt > FLT_EPSILON) { + if (!_current_filter_seeded) { + _current_filter.reset(fmaxf(current_a, 0.f)); + _current_filter_seeded = true; + } + current_a = _current_filter.update(fmaxf(current_a, 0.f), dt); } } @@ -146,11 +156,29 @@ AnalogBattery::updateParams() param_get(_analog_param_handles.i_filt, &_analog_params.i_filt); if (_analog_params.v_filt > FLT_EPSILON) { + const float voltage_state = _voltage_filter.getState(); _voltage_filter = AlphaFilter(_analog_params.v_filt); + + if (_voltage_filter_seeded) { + _voltage_filter.reset(voltage_state); + } + + } else { + // reseed from the next sample if the filter is enabled again + _voltage_filter_seeded = false; } if (_analog_params.i_filt > FLT_EPSILON) { + const float current_state = _current_filter.getState(); _current_filter = AlphaFilter(_analog_params.i_filt); + + if (_current_filter_seeded) { + _current_filter.reset(current_state); + } + + } else { + // reseed from the next sample if the filter is enabled again + _current_filter_seeded = false; } Battery::updateParams(); diff --git a/src/modules/battery_status/analog_battery.h b/src/modules/battery_status/analog_battery.h index 8659adac05f4..d1d07463032c 100644 --- a/src/modules/battery_status/analog_battery.h +++ b/src/modules/battery_status/analog_battery.h @@ -98,4 +98,6 @@ class AnalogBattery : public Battery hrt_abstime _last_timestamp{0}; AlphaFilter _voltage_filter{}; AlphaFilter _current_filter{}; + bool _voltage_filter_seeded{false}; + bool _current_filter_seeded{false}; };