From b8877c4cfc9c22c7ee90efa8dc5a2a582825b297 Mon Sep 17 00:00:00 2001 From: Alex Klimaj Date: Wed, 18 Feb 2026 20:20:50 -0700 Subject: [PATCH] Battery add configurable filter params (#26524) * ark fpv board_config: update battery ADC current filter time constant to 0.5s * battery: add configurable voltage and current filter time constants --------- Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> --- boards/ark/fpv/init/rc.board_defaults | 2 + boards/ark/fpv/src/board_config.h | 3 -- src/modules/battery_status/analog_battery.cpp | 44 +++++++++++++------ src/modules/battery_status/analog_battery.h | 18 +++----- src/modules/battery_status/module.yaml | 34 ++++++++++++++ 5 files changed, 73 insertions(+), 28 deletions(-) diff --git a/boards/ark/fpv/init/rc.board_defaults b/boards/ark/fpv/init/rc.board_defaults index dc7a548573..becc9e8979 100644 --- a/boards/ark/fpv/init/rc.board_defaults +++ b/boards/ark/fpv/init/rc.board_defaults @@ -26,6 +26,8 @@ then fi param set-default BAT1_V_DIV 21.0 +param set-default BAT1_V_FILT 0.075 +param set-default BAT1_I_FILT 0.5 param set-default RC_CRSF_PRT_CFG 300 param set-default RC_SBUS_PRT_CFG 0 diff --git a/boards/ark/fpv/src/board_config.h b/boards/ark/fpv/src/board_config.h index 11bbc5d91a..04e1cc03ec 100644 --- a/boards/ark/fpv/src/board_config.h +++ b/boards/ark/fpv/src/board_config.h @@ -176,9 +176,6 @@ #define BOARD_BATTERY1_V_DIV (21.0f) // (20k + 1k) / 1k = 21 -#define BOARD_BATTERY_ADC_VOLTAGE_FILTER_S 0.075f -#define BOARD_BATTERY_ADC_CURRENT_FILTER_S 0.125f - #define ADC_SCALED_PAYLOAD_SENSE ADC_SCALED_12V_CHANNEL /* HW has to large of R termination on ADC todo:change when HW value is chosen */ diff --git a/src/modules/battery_status/analog_battery.cpp b/src/modules/battery_status/analog_battery.cpp index 05fa264abf..3a1357b868 100644 --- a/src/modules/battery_status/analog_battery.cpp +++ b/src/modules/battery_status/analog_battery.cpp @@ -32,6 +32,7 @@ ****************************************************************************/ #include +#include #include #include "analog_battery.h" @@ -71,6 +72,12 @@ AnalogBattery::AnalogBattery(int index, ModuleParams *parent, const int sample_i snprintf(param_name, sizeof(param_name), "BAT%d_I_OVERWRITE", index); _analog_param_handles.i_overwrite = param_find(param_name); + + snprintf(param_name, sizeof(param_name), "BAT%d_V_FILT", index); + _analog_param_handles.v_filt = param_find(param_name); + + snprintf(param_name, sizeof(param_name), "BAT%d_I_FILT", index); + _analog_param_handles.i_filt = param_find(param_name); } void @@ -81,24 +88,23 @@ AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, (BOARD_ADC_OPEN_CIRCUIT_V <= BOARD_VALID_UV || is_valid()); float current_a = (current_raw - _analog_params.v_offs_cur) * _analog_params.a_per_v; -#if defined(BOARD_BATTERY_ADC_VOLTAGE_FILTER_S) || defined(BOARD_BATTERY_ADC_CURRENT_FILTER_S) + if (_analog_params.v_filt > FLT_EPSILON || _analog_params.i_filt > FLT_EPSILON) { + if (_last_timestamp == 0) { + _last_timestamp = timestamp; + } - if (_last_timestamp == 0) { + const float dt = (timestamp - _last_timestamp) / 1e6f; _last_timestamp = timestamp; + + if (_analog_params.v_filt > FLT_EPSILON) { + voltage_v = _voltage_filter.update(fmaxf(voltage_v, 0.f), dt); + } + + if (_analog_params.i_filt > FLT_EPSILON) { + current_a = _current_filter.update(fmaxf(current_a, 0.f), dt); + } } - const float dt = (timestamp - _last_timestamp) / 1e6f; - _last_timestamp = timestamp; -#endif - -#ifdef BOARD_BATTERY_ADC_VOLTAGE_FILTER_S - voltage_v = _voltage_filter.update(fmaxf(voltage_v, 0.f), dt); -#endif - -#ifdef BOARD_BATTERY_ADC_CURRENT_FILTER_S - current_a = _current_filter.update(fmaxf(current_a, 0.f), dt); -#endif - // Overwrite the measured current if current overwrite is defined and vehicle is unarmed if (_analog_params.i_overwrite > 0) { updateTopics(); @@ -158,6 +164,16 @@ AnalogBattery::updateParams() param_get(_analog_param_handles.i_channel, &_analog_params.i_channel); param_get(_analog_param_handles.i_overwrite, &_analog_params.i_overwrite); param_get(_analog_param_handles.v_offs_cur, &_analog_params.v_offs_cur); + param_get(_analog_param_handles.v_filt, &_analog_params.v_filt); + param_get(_analog_param_handles.i_filt, &_analog_params.i_filt); + + if (_analog_params.v_filt > FLT_EPSILON) { + _voltage_filter = AlphaFilter(_analog_params.v_filt); + } + + if (_analog_params.i_filt > FLT_EPSILON) { + _current_filter = AlphaFilter(_analog_params.i_filt); + } Battery::updateParams(); } diff --git a/src/modules/battery_status/analog_battery.h b/src/modules/battery_status/analog_battery.h index 7cdd5668d8..df308fc710 100644 --- a/src/modules/battery_status/analog_battery.h +++ b/src/modules/battery_status/analog_battery.h @@ -80,6 +80,8 @@ protected: param_t v_channel; param_t i_channel; param_t i_overwrite; + param_t v_filt; + param_t i_filt; } _analog_param_handles; struct { @@ -89,6 +91,8 @@ protected: int32_t v_channel; int32_t i_channel; float i_overwrite; + float v_filt; + float i_filt; } _analog_params; virtual void updateParams() override; @@ -100,17 +104,9 @@ private: uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uint8_t _arming_state{0}; -#if defined(BOARD_BATTERY_ADC_VOLTAGE_FILTER_S) || defined(BOARD_BATTERY_ADC_CURRENT_FILTER_S) - hrt_abstime _last_timestamp {0}; -#endif - -#ifdef BOARD_BATTERY_ADC_VOLTAGE_FILTER_S - AlphaFilter _voltage_filter {BOARD_BATTERY_ADC_VOLTAGE_FILTER_S}; -#endif - -#ifdef BOARD_BATTERY_ADC_CURRENT_FILTER_S - AlphaFilter _current_filter {BOARD_BATTERY_ADC_CURRENT_FILTER_S}; -#endif + hrt_abstime _last_timestamp{0}; + AlphaFilter _voltage_filter{}; + AlphaFilter _current_filter{}; void updateTopics(); }; diff --git a/src/modules/battery_status/module.yaml b/src/modules/battery_status/module.yaml index c92cce7290..03fa372ba1 100644 --- a/src/modules/battery_status/module.yaml +++ b/src/modules/battery_status/module.yaml @@ -82,3 +82,37 @@ parameters: num_instances: *max_num_config_instances instance_start: 1 default: [0, 0] + + BAT${i}_V_FILT: + description: + short: Battery ${i} voltage filter time constant + long: | + Low-pass filter time constant for the battery voltage ADC reading (in seconds). + A higher value results in more smoothing and less noise, but slower response. + A value of 0 disables the filter. + + type: float + decimal: 3 + min: 0.0 + max: 5.0 + unit: s + num_instances: *max_num_config_instances + instance_start: 1 + default: [0.0, 0.0] + + BAT${i}_I_FILT: + description: + short: Battery ${i} current filter time constant + long: | + Low-pass filter time constant for the battery current ADC reading (in seconds). + A higher value results in more smoothing and less noise, but slower response. + A value of 0 disables the filter. + + type: float + decimal: 3 + min: 0.0 + max: 5.0 + unit: s + num_instances: *max_num_config_instances + instance_start: 1 + default: [0.0, 0.0]