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 =