Manual idle current setting (#23914)

* Added param to overwrite analog idle current measurement * Fixed topic handling
PX4 · Nov 12, 2024 · 5cd3a84 · 5cd3a84
1 parent 198014f
commit 5cd3a84
Show file tree

Hide file tree

Showing 3 changed files with 51 additions and 2 deletions.
diff --git a/src/modules/battery_status/analog_battery.cpp b/src/modules/battery_status/analog_battery.cpp
@@ -68,16 +68,27 @@ AnalogBattery::AnalogBattery(int index, ModuleParams *parent, const int sample_i
 
 	snprintf(param_name, sizeof(param_name), "BAT%d_I_CHANNEL", index);
 	_analog_param_handles.i_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);
 }
 
 void
 AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, float current_raw)
 {
 	const float voltage_v = voltage_raw * _analog_params.v_div;
-	const float current_a = (current_raw - _analog_params.v_offs_cur) * _analog_params.a_per_v;
-
 	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;
+
+	// 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);
@@ -123,7 +134,17 @@ AnalogBattery::updateParams()
 	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_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);
 
 	Battery::updateParams();
 }
+
+void AnalogBattery::updateTopics()
+{
+	vehicle_status_s vehicle_status;
+
+	if (_vehicle_status_sub.update(&vehicle_status)) {
+		_arming_state = vehicle_status.arming_state;
+	}
+}
diff --git a/src/modules/battery_status/analog_battery.h b/src/modules/battery_status/analog_battery.h
@@ -35,6 +35,7 @@
 
 #include <battery/battery.h>
 #include <parameters/param.h>
+#include <uORB/topics/vehicle_status.h>
 
 class AnalogBattery : public Battery
 {
@@ -77,6 +78,7 @@ class AnalogBattery : public Battery
 		param_t a_per_v;
 		param_t v_channel;
 		param_t i_channel;
+		param_t i_overwrite;
 	} _analog_param_handles;
 
 	struct {
@@ -85,7 +87,14 @@ class AnalogBattery : public Battery
 		float a_per_v;
 		int32_t v_channel;
 		int32_t i_channel;
+		float i_overwrite;
 	} _analog_params;
 
 	virtual void updateParams() override;
+
+private:
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uint8_t _arming_state{0};
+
+	void updateTopics();
 };
diff --git a/src/modules/battery_status/module.yaml b/src/modules/battery_status/module.yaml
@@ -61,3 +61,22 @@ parameters:
             num_instances: *max_num_config_instances
             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]