battery: improve flight time remaining improvements

- introduce BAT_AVRG_CURRENT param that is used for init of average current estimate
- increase filtering of average current estimation
- only update average current filter when armed

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

src/lib/battery/battery.cpp

@@ -55,7 +55,8 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us,
 	const float expected_filter_dt = static_cast<float>(sample_interval_us) / 1_s;
 	_voltage_filter_v.setParameters(expected_filter_dt, 1.f);
 	_current_filter_a.setParameters(expected_filter_dt, .5f);
-	_current_average_filter_a.setParameters(expected_filter_dt, 25.f);
+	_current_average_filter_a.setParameters(expected_filter_dt, 50.f);
+
 	_throttle_filter.setParameters(expected_filter_dt, 1.f);
 
 	if (index > 9 || index < 1) {
@@ -94,6 +95,8 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us,
 	_param_handles.crit_thr = param_find("BAT_CRIT_THR");
 	_param_handles.emergen_thr = param_find("BAT_EMERGEN_THR");
 
+	_param_handles.bat_avrg_current = param_find("BAT_AVRG_CURRENT");
+
 	updateParams();
 }
 
@@ -268,25 +271,25 @@ void Battery::computeScale()
 
 float Battery::computeRemainingTime(float current_a)
 {
-	float time_remaining_s{NAN};
+	float time_remaining_s = NAN;
 
-	// Only estimate remaining time with useful in flight current measurements
-	if (_current_filter_a.getState() > 1.f) {
-		// Initialize strongly filtered current to an estimated average consumption
-		if (_current_average_filter_a.getState() < 0.f) {
-			// TODO: better initial value based on "average current" from last flight
-			_current_average_filter_a.reset(15.f);
+	// Remaining time estimation only possible with capacity
+	if (_params.capacity > 0.f) {
+		vehicle_status_s vehicle_status{};
+		_vehicle_status_sub.copy(&vehicle_status);
+
+		if (!PX4_ISFINITE(_current_average_filter_a.getState()) || _current_average_filter_a.getState() < FLT_EPSILON) {
+			_current_average_filter_a.reset(_params.bat_avrg_current);
 		}
 
-		// Filter current very strong, we basically want the average consumption
-		_current_average_filter_a.update(current_a);
-
-		// Remaining time estimation only possible with capacity
-		if (_params.capacity > 0.f) {
-			const float remaining_capacity_mah = _state_of_charge * _params.capacity;
-			const float current_ma = _current_average_filter_a.getState() * 1e3f;
-			time_remaining_s = remaining_capacity_mah / current_ma * 3600.f;
+		if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED && PX4_ISFINITE(current_a)) {
+			// only update with positive numbers
+			_current_average_filter_a.update(fmaxf(current_a, 0.f));
 		}
+
+		const float remaining_capacity_mah = _state_of_charge * _params.capacity;
+		const float current_ma = fmaxf(_current_average_filter_a.getState() * 1e3f, FLT_EPSILON);
+		time_remaining_s = remaining_capacity_mah / current_ma * 3600.f;
 	}
 
 	return time_remaining_s;
@@ -304,6 +307,7 @@ void Battery::updateParams()
 	param_get(_param_handles.low_thr, &_params.low_thr);
 	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);
 
 	ModuleParams::updateParams();
 

src/lib/battery/battery.h

@@ -57,6 +57,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/battery_status.h>
+#include <uORB/topics/vehicle_status.h>
 
 /**
  * BatteryBase is a base class for any type of battery.
@@ -119,6 +120,7 @@ protected:
 		param_t crit_thr;
 		param_t emergen_thr;
 		param_t source;
+		param_t bat_avrg_current;
 	} _param_handles{};
 
 	struct {
@@ -132,6 +134,7 @@ protected:
 		float crit_thr;
 		float emergen_thr;
 		int32_t source;
+		float bat_avrg_current;
 	} _params{};
 
 	const int _index;
@@ -147,6 +150,7 @@ private:
 	float computeRemainingTime(float current_a);
 
 	uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	uORB::PublicationMulti<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};
 
 	bool _connected{false};

src/lib/battery/battery_params_common.c

@@ -85,3 +85,17 @@ PARAM_DEFINE_FLOAT(BAT_CRIT_THR, 0.07f);
  * @increment 0.01
  */
 PARAM_DEFINE_FLOAT(BAT_EMERGEN_THR, 0.05f);
+
+/**
+ * Expected battery current in flight.
+ *
+ * This value is used to initialize the in-flight average current estimation,
+ * which in turn is used for estimating remaining flight time and RTL triggering.
+ *
+ * @group Battery Calibration
+ * @unit A
+ * @min 0
+ * @max 500
+ * @increment 0.1
+ */
+PARAM_DEFINE_FLOAT(BAT_AVRG_CURRENT, 15.0f);