From 56fcabf0b1e489b67f7411bb48105efa6b5afbdb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 26 Aug 2023 12:57:45 +1200 Subject: [PATCH] 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 --- .../mavlink/streams/BATTERY_STATUS.hpp | 24 +++++++++++++++++-- 1 file changed, 22 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/streams/BATTERY_STATUS.hpp b/src/modules/mavlink/streams/BATTERY_STATUS.hpp index 1f8248bd81..389631db87 100644 --- a/src/modules/mavlink/streams/BATTERY_STATUS.hpp +++ b/src/modules/mavlink/streams/BATTERY_STATUS.hpp @@ -154,9 +154,29 @@ private: } 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(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 =