fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: fix BATTERY_STATUS extension

Browse Source 
The extension fields need to be 0 by default according to the MAVLink
spec. This is because extensions are 0 by default and need to be 0 when
unknown/unused for backwards compatibility.

The patch also simplifies the flow slightly in that it doesn't create a
temporary array but just fills in the cell voltages directly.

Signed-off-by: Julian Oes <julian@oes.ch>
This commit is contained in:
Julian Oes 2023-08-26 11:58:43 +12:00 committed by Matthias Grob
parent 0f260fc26c
commit 74b4902e50
1 changed files with 17 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -142,16 +142,21 @@ private:
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.
if (battery_status.cell_count == 0 || battery_status.voltage_cell_v[0] < 0.0001f) {
cell_voltages[0] = battery_status.voltage_filtered_v * 1000.f;
bat_msg.voltages[0] = battery_status.voltage_filtered_v * 1000.f;
} else {
static constexpr int uorb_cell_slots =
@ -161,22 +166,17 @@ private:
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;
}