feat(battery): enable use of BAT_n_I_OVERWRITE for all battery estimation sources

Signed-off-by: Silvan <silvan@auterion.com>

Author: sfuhrer

src/lib/battery/battery.cpp

@@ -87,6 +87,9 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us,
 	snprintf(param_name, sizeof(param_name), "BAT%d_SOURCE", _index);
 	_param_handles.source = param_find(param_name);
 
+	snprintf(param_name, sizeof(param_name), "BAT%d_I_OVERWRITE", _index);
+	_param_handles.i_overwrite = param_find(param_name);
+
 	_param_handles.low_thr = param_find("BAT_LOW_THR");
 	_param_handles.crit_thr = param_find("BAT_CRIT_THR");
 	_param_handles.emergen_thr = param_find("BAT_EMERGEN_THR");
@@ -103,7 +106,13 @@ void Battery::updateVoltage(const float voltage_v)
 
 void Battery::updateCurrent(const float current_a)
 {
-	_current_a = current_a;
+	// Overwrite the measured current if current overwrite is defined and vehicle is unarmed
+	if (!_armed && _params.i_overwrite > FLT_EPSILON) {
+		_current_a = _params.i_overwrite;
+
+	} else {
+		_current_a = current_a;
+	}
 }
 
 void Battery::updateTemperature(const float temperature_c)
@@ -144,6 +153,14 @@ void Battery::updateBatteryStatus(const hrt_abstime &timestamp)
 	if (_connected && _battery_initialized) {
 		_warning = determineWarning(_state_of_charge);
 	}
+
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status;
+
+		if (_vehicle_status_sub.copy(&vehicle_status)) {
+			_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+		}
+	}
 }
 
 battery_status_s Battery::getBatteryStatus()
@@ -359,7 +376,6 @@ float Battery::computeRemainingTime(float current_a)
 		vehicle_status_s vehicle_status;
 
 		if (_vehicle_status_sub.copy(&vehicle_status)) {
-			_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
 
 			if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !_vehicle_status_is_fw) {
 				reset_current_avg_filter = true;
@@ -412,6 +428,7 @@ void Battery::updateParams()
 	param_get(_param_handles.crit_thr, &_params.crit_thr);
 	param_get(_param_handles.emergen_thr, &_params.emergen_thr);
 	param_get(_param_handles.bat_avrg_current, &_params.bat_avrg_current);
+	param_get(_param_handles.i_overwrite, &_params.i_overwrite);
 
 	float capacity{0.f};
 	param_get(_param_handles.capacity, &capacity);

src/lib/battery/battery.h

@@ -147,6 +147,7 @@ class Battery : public ModuleParams
 		param_t emergen_thr;
 		param_t source;
 		param_t bat_avrg_current;
+		param_t i_overwrite;
 	} _param_handles{};
 
 	struct {
@@ -159,6 +160,7 @@ class Battery : public ModuleParams
 		float emergen_thr;
 		int32_t source;
 		float bat_avrg_current;
+		float i_overwrite;
 	} _params{};
 
 	const int _index;

src/lib/battery/module.yaml

@@ -126,6 +126,23 @@ parameters:
             instance_start: 1
             default: [0, -1, -1]
 
+        BAT${i}_I_OVERWRITE:
+            description:
+                short: Battery ${i} idle current overwrite
+                long: |
+                    This parameter allows to overwrite the current measured during
+                    idle (unarmed) state with a user-defined constant value (expressed in amperes).
+                    When the system is armed, the measured current is used. This is useful
+                    because on certain ESCs current measurements are inaccurate in case of no load.
+                    Negative values are ignored and will cause the measured current to be used.
+                    The default value of 0 disables the overwrite, in which case the measured value
+                    is always used.
+            type: float
+            decimal: 2
+            num_instances: *max_num_config_instances
+            instance_start: 1
+            default: [0, 0, 0]
+
         BAT_LOW_THR:
             description:
                 short: Low threshold.

src/modules/battery_status/analog_battery.cpp

@@ -67,9 +67,6 @@ AnalogBattery::AnalogBattery(int index, ModuleParams *parent, const int sample_i
 	snprintf(param_name, sizeof(param_name), "BAT%d_V_CHANNEL", index);
 	_analog_param_handles.v_channel = param_find(param_name);
 
-	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);
 
@@ -102,15 +99,6 @@ AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw,
 		}
 	}
 
-	// Overwrite the measured current if current overwrite is defined and vehicle is unarmed
-	if (_analog_params.i_overwrite > 0) {
-		updateTopics();
-
-		if (_arming_state == vehicle_status_s::ARMING_STATE_DISARMED) {
-			current_a = _analog_params.i_overwrite;
-		}
-	}
-
 	Battery::setConnected(connected);
 	Battery::updateVoltage(voltage_v);
 	Battery::updateCurrent(current_a);
@@ -153,7 +141,6 @@ AnalogBattery::updateParams()
 	param_get(_analog_param_handles.v_div, &_analog_params.v_div);
 	param_get(_analog_param_handles.a_per_v, &_analog_params.a_per_v);
 	param_get(_analog_param_handles.v_channel, &_analog_params.v_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);
@@ -168,12 +155,3 @@ AnalogBattery::updateParams()
 
 	Battery::updateParams();
 }
-
-void AnalogBattery::updateTopics()
-{
-	vehicle_status_s vehicle_status;
-
-	if (_vehicle_status_sub.update(&vehicle_status)) {
-		_arming_state = vehicle_status.arming_state;
-	}
-}

src/modules/battery_status/analog_battery.h

@@ -36,8 +36,6 @@
 #include <battery/battery.h>
 #include <lib/mathlib/math/filter/AlphaFilter.hpp>
 #include <parameters/param.h>
-#include <uORB/topics/vehicle_status.h>
-
 class AnalogBattery : public Battery
 {
 public:
@@ -78,7 +76,6 @@ class AnalogBattery : public Battery
 		param_t v_div;
 		param_t a_per_v;
 		param_t v_channel;
-		param_t i_overwrite;
 		param_t v_filt;
 		param_t i_filt;
 	} _analog_param_handles;
@@ -88,7 +85,6 @@ class AnalogBattery : public Battery
 		float v_div;
 		float a_per_v;
 		int32_t v_channel;
-		float i_overwrite;
 		float v_filt;
 		float i_filt;
 	} _analog_params;
@@ -99,12 +95,7 @@ class AnalogBattery : public Battery
 
 	static constexpr int V_CHANNEL_DISABLED = -2;
 
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
-	uint8_t _arming_state{0};
-
 	hrt_abstime _last_timestamp{0};
 	AlphaFilter<float> _voltage_filter{};
 	AlphaFilter<float> _current_filter{};
-
-	void updateTopics();
 };

src/modules/battery_status/module.yaml

@@ -51,25 +51,6 @@ parameters:
             instance_start: 1
             default: [-1, -1]
 
-        BAT${i}_I_OVERWRITE:
-            description:
-                short: Battery ${i} idle current overwrite
-                long: |
-                    This parameter allows to overwrite the current measured during
-                    idle (unarmed) state with a user-defined constant value (expressed in amperes).
-                    When the system is armed, the measured current is used. This is useful
-                    because on certain ESCs current measurements are inaccurate in case of no load.
-                    Negative values are ignored and will cause the measured current to be used.
-                    The default value of 0 disables the overwrite, in which case the measured value
-                    is always used.
-
-            type: float
-            decimal: 8
-            reboot_required: true
-            num_instances: *max_num_config_instances
-            instance_start: 1
-            default: [0, 0]
-
         BAT${i}_V_FILT:
             description:
                 short: Battery ${i} voltage filter time constant