battery: improve remaining flight time estimation

makes the remaining flight time estimation based on SoC and it's derivative  s.t. it also works without setting battery capacity

src/lib/battery/battery.cpp

@@ -57,6 +57,8 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us,
 	_current_average_filter_a.setParameters(expected_filter_dt, 50.f);
 	_ocv_filter_v.setParameters(expected_filter_dt, 1.f);
 	_cell_voltage_filter_v.setParameters(expected_filter_dt, 1.f);
+	_soc_derivative_filter.setParameters(expected_filter_dt, 50.f);
+	_flight_time_filter.setParameters(expected_filter_dt, 50.f);
 
 	if (index > 9 || index < 1) {
 		PX4_ERR("Battery index must be between 1 and 9 (inclusive). Received %d. Defaulting to 1.", index);
@@ -92,6 +94,7 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us,
 	_param_handles.emergen_thr = param_find("BAT_EMERGEN_THR");
 
 	_param_handles.bat_avrg_current = param_find("BAT_AVRG_CURRENT");
+	_param_handles.bat_avrg_soc = param_find("BAT_AVRG_SOC");
 
 	updateParams();
 }
@@ -148,7 +151,7 @@ battery_status_s Battery::getBatteryStatus()
 	battery_status.discharged_mah = _discharged_mah;
 	battery_status.remaining = _state_of_charge;
 	battery_status.scale = _scale;
-	battery_status.time_remaining_s = computeRemainingTime(_current_a);
+	battery_status.time_remaining_s = computeRemainingTime(_state_of_charge);
 	battery_status.temperature = NAN;
 	battery_status.cell_count = _params.n_cells;
 	battery_status.connected = _connected;
@@ -340,10 +343,11 @@ void Battery::computeScale()
 	}
 }
 
-float Battery::computeRemainingTime(float current_a)
+float Battery::computeRemainingTime(const float state_of_charge)
 {
 	float time_remaining_s = NAN;
-	bool reset_current_avg_filter = false;
+	bool reset_soc_derivative_filter = false;
+	hrt_abstime timestamp = hrt_absolute_time();
 
 	if (_vehicle_status_sub.updated()) {
 		vehicle_status_s vehicle_status;
@@ -352,7 +356,7 @@ float Battery::computeRemainingTime(float current_a)
 			_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;
+				reset_soc_derivative_filter = true;
 			}
 
 			_vehicle_status_is_fw = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
@@ -362,28 +366,31 @@ float Battery::computeRemainingTime(float current_a)
 	_flight_phase_estimation_sub.update();
 
 	// reset filter if not feasible, negative or we did a VTOL transition to FW mode
-	if (!PX4_ISFINITE(_current_average_filter_a.getState()) || _current_average_filter_a.getState() < FLT_EPSILON
-	    || reset_current_avg_filter) {
-		_current_average_filter_a.reset(_params.bat_avrg_current);
+	if (!PX4_ISFINITE(_soc_derivative_filter.getState()) || _soc_derivative_filter.getState() < FLT_EPSILON
+	    || reset_soc_derivative_filter) {
+		_soc_derivative_filter.reset(_params.bat_avrg_soc);
 	}
 
-	if (_armed && PX4_ISFINITE(current_a)) {
+	const float dt = math::constrain(timestamp - _last_flight_time_update, 1_ms, 5000_ms) * 1e-6f;
+
+	if (_armed && _state_of_charge < 0.99f && PX4_ISFINITE(dt)) {
 		// For FW only update when we are in level flight
-		if (!_vehicle_status_is_fw || ((hrt_absolute_time() - _flight_phase_estimation_sub.get().timestamp) < 2_s
+		if (!_vehicle_status_is_fw || ((timestamp - _flight_phase_estimation_sub.get().timestamp) < 2_s
 					       && _flight_phase_estimation_sub.get().flight_phase == flight_phase_estimation_s::FLIGHT_PHASE_LEVEL)) {
-			// only update with positive numbers
-			_current_average_filter_a.update(fmaxf(current_a, 0.f));
+			_soc_derivative_filter.update((_prev_state_of_charge - _state_of_charge) / dt);
 		}
-	}
 
-	// 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 = fmaxf(_current_average_filter_a.getState() * 1e3f, FLT_EPSILON);
-		time_remaining_s = remaining_capacity_mah / current_ma * 3600.f;
+	} else {
+		_soc_derivative_filter.reset(_params.bat_avrg_soc);
+		_flight_time_filter.reset(_state_of_charge / fmaxf(_soc_derivative_filter.getState(), FLT_EPSILON));
 	}
 
-	return time_remaining_s;
+	time_remaining_s = _state_of_charge / fmaxf(_soc_derivative_filter.getState(), FLT_EPSILON);
+	_flight_time_filter.update(time_remaining_s);
+	_last_flight_time_update = timestamp;
+	_prev_state_of_charge = state_of_charge;
+
+	return _flight_time_filter.getState();
 }
 
 void Battery::updateParams()
@@ -399,6 +406,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.bat_avrg_soc, &_params.bat_avrg_soc);
 
 	if (n_cells != _params.n_cells) {
 		_internal_resistance_initialized = false;

src/lib/battery/battery.h

@@ -123,6 +123,7 @@ class Battery : public ModuleParams
 		param_t emergen_thr;
 		param_t source;
 		param_t bat_avrg_current;
+		param_t bat_avrg_soc;
 	} _param_handles{};
 
 	struct {
@@ -136,6 +137,7 @@ class Battery : public ModuleParams
 		float emergen_thr;
 		int32_t source;
 		float bat_avrg_current;
+		float bat_avrg_soc;
 	} _params{};
 
 	const int _index;
@@ -150,7 +152,7 @@ class Battery : public ModuleParams
 	uint8_t determineWarning(float state_of_charge);
 	uint16_t determineFaults();
 	void computeScale();
-	float computeRemainingTime(float current_a);
+	float computeRemainingTime(const float state_of_charge);
 
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	uORB::SubscriptionData<flight_phase_estimation_s> _flight_phase_estimation_sub{ORB_ID(flight_phase_estimation)};
@@ -165,19 +167,23 @@ class Battery : public ModuleParams
 	float _voltage_v{0.f};
 	AlphaFilter<float> _ocv_filter_v;
 	AlphaFilter<float> _cell_voltage_filter_v;
+	AlphaFilter<float> _soc_derivative_filter;
+	AlphaFilter<float> _flight_time_filter;
 	float _current_a{-1};
 	AlphaFilter<float>
 	_current_average_filter_a; ///< averaging filter for current. For FW, it is the current in level flight.
 	float _discharged_mah{0.f};
 	float _discharged_mah_loop{0.f};
 	float _state_of_charge_volt_based{-1.f}; // [0,1]
 	float _state_of_charge{-1.f}; // [0,1]
+	float _prev_state_of_charge{-1.f}; // [0, 1]
 	float _scale{1.f};
 	uint8_t _warning{battery_status_s::BATTERY_WARNING_NONE};
 	hrt_abstime _last_timestamp{0};
 	bool _armed{false};
 	bool _vehicle_status_is_fw{false};
 	hrt_abstime _last_unconnected_timestamp{0};
+	hrt_abstime _last_flight_time_update{0};
 
 	// Internal Resistance estimation
 	void updateInternalResistanceEstimation(const float voltage_v, const float current_a);

src/lib/battery/module.yaml

@@ -178,3 +178,16 @@ parameters:
             decimal: 2
             increment: 0.1
             default: 15
+
+        BAT_AVRG_SOC:
+            description:
+                short: Expected average SoC derivative during flight.
+                long: |
+                    This value is used to initialize the in-flight average SoC derivative,
+                    which in turn is used for estimating remaining flight time and RTL triggering.
+            type: float
+            min: 0
+            max: 100
+            decimal: 4
+            increment: 0.0001
+            default: 0.001