analog_battery: add option for filter

PX4 · Dec 18, 2024 · 3dfb36e · 3dfb36e
1 parent 8a93913
commit 3dfb36e
Show file tree

Hide file tree

Showing 4 changed files with 51 additions and 3 deletions.
diff --git a/boards/ark/fpv/src/board_config.h b/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 */

diff --git a/src/lib/mathlib/math/filter/AlphaFilter.hpp b/src/lib/mathlib/math/filter/AlphaFilter.hpp
@@ -62,8 +62,8 @@ class AlphaFilter
 	 *
 	 * 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)
 	{

diff --git a/src/modules/battery_status/analog_battery.cpp b/src/modules/battery_status/analog_battery.cpp
@@ -71,16 +71,48 @@ 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);
+
+#if defined(BOARD_BATTERY_ADC_VOLTAGE_FILTER_S) || defined(BOARD_BATTERY_ADC_CURRENT_FILTER_S)
+	const float expected_filter_dt = static_cast<float>(sample_interval_us) / 1e6f;
+#endif
+
+#ifdef BOARD_BATTERY_ADC_VOLTAGE_FILTER_S
+	_voltage_filter.setParameters(expected_filter_dt, BOARD_BATTERY_ADC_VOLTAGE_FILTER_S);
+#endif
+
+#ifdef BOARD_BATTERY_ADC_CURRENT_FILTER_S
+	_current_filter.setParameters(expected_filter_dt, BOARD_BATTERY_ADC_CURRENT_FILTER_S);
+#endif
 }
 
 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_filter.update(fmaxf(voltage_v, 0.f), dt);
+	voltage_v = _voltage_filter.getState();
+#endif
+
+#ifdef BOARD_BATTERY_ADC_CURRENT_FILTER_S
+	_current_filter.update(fmaxf(current_a, 0.f), dt);
+	current_a = _current_filter.getState();
+#endif
+
 	// Overwrite the measured current if current overwrite is defined and vehicle is unarmed
 	if (_analog_params.i_overwrite > 0) {
 		updateTopics();

diff --git a/src/modules/battery_status/analog_battery.h b/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 @@ class AnalogBattery : public Battery
 	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;
+#endif
+
+#ifdef BOARD_BATTERY_ADC_CURRENT_FILTER_S
+	AlphaFilter<float> _current_filter;
+#endif
+
 	void updateTopics();
 };