From 98fb37359068a75ca316170149eed5c55b90c2aa Mon Sep 17 00:00:00 2001 From: CUAVmengxiao Date: Fri, 4 Jun 2021 15:20:22 +0800 Subject: [PATCH] mavlink: 14S battery support --- src/modules/mavlink/streams/BATTERY_STATUS.hpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/modules/mavlink/streams/BATTERY_STATUS.hpp b/src/modules/mavlink/streams/BATTERY_STATUS.hpp index 6af59d79b5..c0fefed858 100644 --- a/src/modules/mavlink/streams/BATTERY_STATUS.hpp +++ b/src/modules/mavlink/streams/BATTERY_STATUS.hpp @@ -125,6 +125,17 @@ private: } } + static constexpr int mavlink_cells_ext_max = (sizeof(bat_msg.voltages_ext) / sizeof(bat_msg.voltages_ext[0])); + + for (int cell = mavlink_cells_max; cell < mavlink_cells_max + mavlink_cells_ext_max; cell++) { + if (battery_status.connected && (cell < battery_status.cell_count) && (cell < uorb_cells_max)) { + bat_msg.voltages_ext[cell - mavlink_cells_max] = battery_status.voltage_cell_v[cell] * 1000.0f; + + } else { + bat_msg.voltages_ext[cell - mavlink_cells_max] = UINT16_MAX; + } + } + mavlink_msg_battery_status_send_struct(_mavlink->get_channel(), &bat_msg); updated = true; }