Estimate remaining flight time

msg/battery_status.msg

@@ -8,6 +8,7 @@ float32 current_average_a	# Battery current average in amperes, -1 if unknown
 float32 discharged_mah		# Discharged amount in mAh, -1 if unknown
 float32 remaining			# From 1 to 0, -1 if unknown
 float32 scale				# Power scaling factor, >= 1, or -1 if unknown
+float32 time_remaining_s	# predicted time in seconds remaining until battery is empty under previous averaged load, NAN if unknown
 float32 temperature			# temperature of the battery. NaN if unknown
 int32 cell_count			# Number of cells
 
@@ -18,7 +19,6 @@ uint8 source				# Battery source
 uint8 priority				# Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1
 uint16 capacity				# actual capacity of the battery
 uint16 cycle_count			# number of discharge cycles the battery has experienced
-uint16 run_time_to_empty	# predicted remaining battery capacity based on the present rate of discharge in min
 uint16 average_time_to_empty	# predicted remaining battery capacity based on the average rate of discharge in min
 uint16 serial_number		# serial number of the battery pack
 uint16 manufacture_date		# manufacture date, part of serial number of the battery pack. formated as: Day + Month×32 + (Year–1980)×512

src/drivers/batt_smbus/batt_smbus.cpp

@@ -136,7 +136,7 @@ void BATT_SMBUS::RunImpl()
 
 	// Read run time to empty (minutes).
 	ret |= _interface->read_word(BATT_SMBUS_RUN_TIME_TO_EMPTY, result);
-	new_report.run_time_to_empty = result;
+	new_report.time_remaining_s = result * 60;
 
 	// Read average time to empty (minutes).
 	ret |= _interface->read_word(BATT_SMBUS_AVERAGE_TIME_TO_EMPTY, result);

src/drivers/smart_battery/batmon/batmon.cpp

@@ -162,7 +162,7 @@ void Batmon::RunImpl()
 
 	// Read run time to empty (minutes).
 	ret |= _interface->read_word(BATT_SMBUS_RUN_TIME_TO_EMPTY, result);
-	new_report.run_time_to_empty = result;
+	new_report.time_remaining_s = result * 60;
 
 	// Read average time to empty (minutes).
 	ret |= _interface->read_word(BATT_SMBUS_AVERAGE_TIME_TO_EMPTY, result);

src/drivers/uavcan/sensors/battery.cpp

@@ -83,7 +83,7 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
 	// battery.priority = msg.;
 	battery.capacity = msg.full_charge_capacity_wh;
 	// battery.cycle_count = msg.;
-	// battery.run_time_to_empty = msg.;
+	// battery.time_remaining_s = msg.;
 	// battery.average_time_to_empty = msg.;
 	battery.serial_number = msg.model_instance_id;
 	battery.id = msg.getSrcNodeID().get();

src/drivers/uavcan/sensors/cbat.cpp

@@ -79,7 +79,7 @@ UavcanCBATBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<cuav::equip
 	// battery.priority = msg.;
 	battery.capacity = msg.full_charge_capacity * 1000;
 	battery.cycle_count = msg.cycle_count;
-	battery.run_time_to_empty = msg.average_time_to_empty;
+	battery.time_remaining_s = msg.average_time_to_empty * 60;
 	battery.average_time_to_empty = msg.average_time_to_empty;
 	battery.serial_number = msg.serial_number;
 	battery.manufacture_date = msg.manufacture_date;

src/drivers/uavcan_v1/Subscribers/legacy/LegacyBatteryInfo.hpp

@@ -107,7 +107,7 @@ class UavcanLegacyBatteryInfoSubscriber : public UavcanDynamicPortSubscriber
 		 * connected (partly)
 		 * priority
 		 * cycle_count
-		 * run_time_to_empty
+		 * time_remaining_s
 		 * average_time_to_empty
 		 * manufacture_date
 		 * max_error

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) {
@@ -127,10 +128,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));
@@ -245,6 +247,32 @@ void Battery::computeScale()
 	}
 }
 
+float Battery::computeRemainingTime(float current_a)
+{
+	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);
+		}
+
+		// 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()
 {
 	param_get(_param_handles.v_empty, &_params.v_empty);

src/lib/battery/battery.h

@@ -133,17 +133,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};

src/modules/mavlink/streams/BATTERY_STATUS.hpp

@@ -76,7 +76,9 @@ class MavlinkStreamBatteryStatus : public MavlinkStream
 				bat_msg.energy_consumed = -1;
 				bat_msg.current_battery = (battery_status.connected) ? battery_status.current_filtered_a * 100 : -1;
 				bat_msg.battery_remaining = (battery_status.connected) ? ceilf(battery_status.remaining * 100.f) : -1;
-				bat_msg.time_remaining = (battery_status.connected) ? battery_status.run_time_to_empty * 60 : 0;
+				// MAVLink extension: 0 is unsupported, in uORB it's NAN
+				bat_msg.time_remaining = (battery_status.connected && (PX4_ISFINITE(battery_status.time_remaining_s))) ?
+							 math::max((int)battery_status.time_remaining_s, 1) : 0;
 
 				switch (battery_status.warning) {
 				case (battery_status_s::BATTERY_WARNING_NONE):