Fix BATTERY_STATUS voltage reporting

src/modules/mavlink/streams/BATTERY_STATUS.hpp

@@ -142,16 +142,41 @@ class MavlinkStreamBatteryStatus : public MavlinkStream
 				static constexpr int mavlink_cell_slots = (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0]));
 				static constexpr int 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;
+				// Fill defaults first, voltage fields 1-10
+				for (int i = 0; i < mavlink_cell_slots; ++i) {
+					bat_msg.voltages[i] = UINT16_MAX;
+				}
+
+				// And extensions fields 11-14: 0 if unused for backwards compatibility and 0 truncation.
+				for (int i = 0; i < mavlink_cell_slots_extension; ++i) {
+					bat_msg.voltages_ext[i] = 0;
 				}
 
 				if (battery_status.connected) {
-					// 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
+					// 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, or cell(s) if the voltage
+					// doesn't "fit" in the UINT16.
 					if (battery_status.cell_count == 0 || battery_status.voltage_cell_v[0] < 0.0001f) {
-						cell_voltages[0] = battery_status.voltage_filtered_v * 1000.f;
+						// If it doesn't fit, we have to split it into UINT16-1 chunks and the remaining
+						// voltage for the subsequent field.
+						// This won't work for voltages of more than 655 volts.
+						const int num_fields_required = static_cast<int>(battery_status.voltage_filtered_v * 1000.f) / (UINT16_MAX - 1) + 1;
+
+						if (num_fields_required <= mavlink_cell_slots) {
+							float remaining_voltage = battery_status.voltage_filtered_v * 1000.f;
+
+							for (int i = 0; i < num_fields_required - 1; ++i) {
+								bat_msg.voltages[i] = UINT16_MAX - 1;
+								remaining_voltage -= UINT16_MAX - 1;
+							}
+
+							bat_msg.voltages[num_fields_required - 1] = remaining_voltage;
+
+						} else {
+							// Leave it default/unknown. We're out of spec.
+						}
+
 
 					} else {
 						static constexpr int uorb_cell_slots =
@@ -161,22 +186,17 @@ class MavlinkStreamBatteryStatus : public MavlinkStream
 										 uorb_cell_slots,
 										 mavlink_cell_slots + mavlink_cell_slots_extension);
 
-						for (int cell = 0; cell < cell_slots; cell++) {
-							cell_voltages[cell] = battery_status.voltage_cell_v[cell] * 1000.f;
+						for (int i = 0; i < cell_slots; ++i) {
+							if (i < mavlink_cell_slots) {
+								bat_msg.voltages[i] = battery_status.voltage_cell_v[i] * 1000.f;
+
+							} else if ((i - mavlink_cell_slots) < mavlink_cell_slots_extension) {
+								bat_msg.voltages_ext[i - mavlink_cell_slots] = battery_status.voltage_cell_v[i] * 1000.f;
+							}
 						}
 					}
 				}
 
-				// voltage fields 1-10
-				for (int cell = 0; cell < mavlink_cell_slots; cell++) {
-					bat_msg.voltages[cell] = cell_voltages[cell];
-				}
-
-				// voltage fields 11-14 into the extension
-				for (int 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);
 				updated = true;
 			}