analog_battery: add option for filter (#23987)

boards/ark/fpv/src/board_config.h

@@ -176,6 +176,9 @@
 
 #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/lib/mathlib/math/filter/AlphaFilter.hpp

@@ -62,8 +62,8 @@ public:
 	 *
 	 * Both parameters have to be provided in the same units.
 	 *
-	 * @param sample_interval interval between two samples
-	 * @param time_constant filter time constant determining convergence
+	 * @param sample_interval interval between two samples in seconds
+	 * @param time_constant filter time constant determining convergence in seconds
 	 */
 	void setParameters(float sample_interval, float time_constant)
 	{

src/modules/battery_status/analog_battery.cpp

@@ -76,11 +76,29 @@ AnalogBattery::AnalogBattery(int index, ModuleParams *parent, const int sample_i
 void
 AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, float current_raw)
 {
-	const float voltage_v = voltage_raw * _analog_params.v_div;
+	float voltage_v = voltage_raw * _analog_params.v_div;
 	const bool connected = voltage_v > BOARD_ADC_OPEN_CIRCUIT_V &&
 			       (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 (_last_timestamp == 0) {
+		_last_timestamp = timestamp;
+	}
+
+	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();

src/modules/battery_status/analog_battery.h

@@ -34,6 +34,7 @@
 #pragma once
 
 #include <battery/battery.h>
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
 #include <parameters/param.h>
 #include <uORB/topics/vehicle_status.h>
 
@@ -96,5 +97,17 @@ 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<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();
 };