mavlink: Support voltages > 65v in battery status

If the measured voltage is more than 65v we need to split the voltage
over multiple cells in order to avoid overflowing the uint16. This is
according to the MAVLink spec.

Signed-off-by: Julian Oes <julian@oes.ch>

src/modules/mavlink/streams/BATTERY_STATUS.hpp

@@ -154,9 +154,29 @@ class MavlinkStreamBatteryStatus : public MavlinkStream
 				}
 
 				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) {
-						bat_msg.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 =