battery: computer remaining flight time

src/lib/battery/battery.cpp

@@ -54,6 +54,7 @@ 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);
 	_throttle_filter.setParameters(expected_filter_dt, 1.f);
 
 	if (index > 9 || index < 1) {
@@ -135,10 +136,11 @@ void Battery::updateBatteryStatus(const hrt_abstime &timestamp, float voltage_v,
 	battery_status.voltage_filtered_v = _voltage_filter_v.getState();
 	battery_status.current_a = current_a;
 	battery_status.current_filtered_a = _current_filter_a.getState();
-	battery_status.current_average_a = -1.f; // support will follow
+	battery_status.current_average_a = _current_average_filter_a.getState();
 	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.temperature = NAN;
 	// Publish at least one cell such that the total voltage gets into MAVLink BATTERY_STATUS
 	battery_status.cell_count = math::max(_params.n_cells, static_cast<int32_t>(1));
@@ -253,6 +255,32 @@ void Battery::computeScale()
 	}
 }
 
+float Battery::computeRemainingTime(float current_a)
+{
+	float time_remaining_s = -1.f;
+
+	// 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);
+		}
+
+		// 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;
+		}
+	}
+
+	return time_remaining_s;
+}
+
 void Battery::updateParams()
 {
 	if (_index == 1) {

src/lib/battery/battery.h

@@ -187,17 +187,19 @@ class Battery : public ModuleParams
 	void estimateStateOfCharge(const float voltage_v, const float current_a, const float throttle);
 	uint8_t determineWarning(float state_of_charge);
 	void computeScale();
+	float computeRemainingTime(float current_a);
 
 	uORB::PublicationMulti<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};
 
 	bool _battery_initialized{false};
 	AlphaFilter<float> _voltage_filter_v;
 	AlphaFilter<float> _current_filter_a;
+	AlphaFilter<float> _current_average_filter_a;
 	AlphaFilter<float> _throttle_filter;
 	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 _state_of_charge_volt_based{-1.f}; // [0,1]
+	float _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};