battery: move MAVLink specific handling out of battery class

msg/battery_status.msg

@@ -10,7 +10,7 @@ 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
+uint8 cell_count			# Number of cells, 0 if unknown
 
 uint8 BATTERY_SOURCE_POWER_MODULE = 0
 uint8 BATTERY_SOURCE_EXTERNAL = 1
@@ -27,7 +27,7 @@ uint16 max_error			# max error, expected margin of error in % in the state-of-ch
 uint8 id					# ID number of a battery. Should be unique and consistent for the lifetime of a vehicle. 1-indexed.
 uint16 interface_error		# interface error counter
 
-float32[14] voltage_cell_v		# Battery individual cell voltages
+float32[14] voltage_cell_v		# Battery individual cell voltages, 0 if unknown
 float32 max_cell_voltage_delta	# Max difference between individual cell voltages
 
 bool is_powering_off			# Power off event imminent indication, false if unknown

src/lib/battery/battery.cpp

@@ -134,8 +134,7 @@ void Battery::updateBatteryStatus(const hrt_abstime &timestamp, float voltage_v,
 	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));
+	battery_status.cell_count = _params.n_cells;
 	battery_status.connected = connected;
 	battery_status.source = source;
 	battery_status.priority = priority;
@@ -143,16 +142,6 @@ void Battery::updateBatteryStatus(const hrt_abstime &timestamp, float voltage_v,
 	battery_status.id = static_cast<uint8_t>(_index);
 	battery_status.warning = _warning;
 
-	static constexpr int32_t uorb_max_cells = sizeof(battery_status.voltage_cell_v) / sizeof(
-				battery_status.voltage_cell_v[0]);
-
-	int max_cells = math::min(battery_status.cell_count, uorb_max_cells);
-
-	// Fill cell voltages with average values to work around MAVLink BATTERY_STATUS not allowing to report just total voltage
-	for (int i = 0; i < max_cells; i++) {
-		battery_status.voltage_cell_v[i] = battery_status.voltage_filtered_v / max_cells;
-	}
-
 	if (source == _params.source) {
 		battery_status.timestamp = hrt_absolute_time();
 		_battery_status_pub.publish(battery_status);

src/modules/mavlink/streams/BATTERY_STATUS.hpp

@@ -114,28 +114,39 @@ private:
 					bat_msg.temperature = INT16_MAX;
 				}
 
-				static constexpr int mavlink_cells_max = (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0]));
-				static constexpr int uorb_cells_max =
-					(sizeof(battery_status.voltage_cell_v) / sizeof(battery_status.voltage_cell_v[0]));
+				// fill cell voltages
+				static constexpr uint8_t mavlink_cell_slots = (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0]));
+				static constexpr uint8_t mavlink_cell_slots_extension
+					= (sizeof(bat_msg.voltages_ext) / sizeof(bat_msg.voltages_ext[0]));
+				uint16_t cell_voltages[mavlink_cell_slots + mavlink_cell_slots_extension];
+
+				for (auto &voltage : cell_voltages) {
+					voltage = UINT16_MAX;
+				}
+
+				// We don't know the cell count or we don't know the indpendent cell voltages so we report the total voltage in the first cell
+				if (battery_status.cell_count == 0 || battery_status.voltage_cell_v[0] < 0.0001f) {
+					cell_voltages[0] = battery_status.voltage_filtered_v * 1000.f;
 
-				for (int cell = 0; cell < mavlink_cells_max; cell++) {
-					if (battery_status.connected && (cell < battery_status.cell_count) && (cell < uorb_cells_max)) {
-						bat_msg.voltages[cell] = battery_status.voltage_cell_v[cell] * 1000.f;
+				} else {
+					static constexpr uint8_t uorb_cell_slots =
+						(sizeof(battery_status.voltage_cell_v) / sizeof(battery_status.voltage_cell_v[0]));
 
-					} else {
-						bat_msg.voltages[cell] = UINT16_MAX;
+					for (uint8_t cell = 0; cell < mavlink_cell_slots + mavlink_cell_slots_extension; cell++) {
+						if (battery_status.connected && cell < math::min(battery_status.cell_count, uorb_cell_slots)) {
+							cell_voltages[cell] = battery_status.voltage_cell_v[cell] * 1000.f;
+						}
 					}
 				}
 
-				static constexpr int mavlink_cells_ext_max = (sizeof(bat_msg.voltages_ext) / sizeof(bat_msg.voltages_ext[0]));
-
-				for (int cell = mavlink_cells_max; cell < mavlink_cells_max + mavlink_cells_ext_max; cell++) {
-					if (battery_status.connected && (cell < battery_status.cell_count) && (cell < uorb_cells_max)) {
-						bat_msg.voltages_ext[cell - mavlink_cells_max] = battery_status.voltage_cell_v[cell] * 1000.0f;
+				// voltage fields 1-10
+				for (uint8_t cell = 0; cell < mavlink_cell_slots; cell++) {
+					bat_msg.voltages[cell] = cell_voltages[cell];
+				}
 
-					} else {
-						bat_msg.voltages_ext[cell - mavlink_cells_max] = UINT16_MAX;
-					}
+				// voltage fields 11-14 into the extension
+				for (uint8_t cell = 0; cell < mavlink_cell_slots_extension; cell++) {
+					bat_msg.voltages_ext[cell] = cell_voltages[mavlink_cell_slots + cell];
 				}
 
 				mavlink_msg_battery_status_send_struct(_mavlink->get_channel(), &bat_msg);