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}; };