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

@@ -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

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 */

src/modules/battery_status/analog_battery.cpp

@@ -32,6 +32,7 @@
  ****************************************************************************/
 
 #include <stdio.h>
+#include <float.h>
 #include <lib/battery/battery.h>
 #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<float>(_analog_params.v_filt);
+	}
+
+	if (_analog_params.i_filt > FLT_EPSILON) {
+		_current_filter = AlphaFilter<float>(_analog_params.i_filt);
+	}
 
 	Battery::updateParams();
 }

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};
-	hrt_abstime _last_timestamp {0};
+	AlphaFilter<float> _voltage_filter{};
-#endif
+	AlphaFilter<float> _current_filter{};
-
-#ifdef BOARD_BATTERY_ADC_VOLTAGE_FILTER_S
-	AlphaFilter<float> _voltage_filter {BOARD_BATTERY_ADC_VOLTAGE_FILTER_S};
-#endif
-
-#ifdef BOARD_BATTERY_ADC_CURRENT_FILTER_S
-	AlphaFilter<float> _current_filter {BOARD_BATTERY_ADC_CURRENT_FILTER_S};
-#endif
 
 	void updateTopics();
 };

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]