dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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>
This commit is contained in:
Julian Oes 2023-08-26 12:57:45 +12:00 committed by Daniel Agar
parent 7b60737f9a
commit 56fcabf0b1
1 changed files with 22 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/modules/mavlink/streams/BATTERY_STATUS.hpp
View File

@ -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<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 =