forked from Archive/PX4-Autopilot
mavlink battery_status: report actual cell voltages
This commit is contained in:
parent
de4f594937
commit
98a3910b06
|
@ -737,10 +737,12 @@ protected:
|
|||
}
|
||||
|
||||
static constexpr int mavlink_cells_max = (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0]));
|
||||
static constexpr int uorb_cells_max =
|
||||
(sizeof(battery_status.voltage_cell_v) / sizeof(battery_status.voltage_cell_v[0]));
|
||||
|
||||
for (int cell = 0; cell < mavlink_cells_max; cell++) {
|
||||
if ((battery_status.cell_count > 0) && (cell < battery_status.cell_count) && battery_status.connected) {
|
||||
bat_msg.voltages[cell] = (battery_status.voltage_v / battery_status.cell_count) * 1000.0f;
|
||||
if (battery_status.connected && (cell < battery_status.cell_count) && (cell < uorb_cells_max)) {
|
||||
bat_msg.voltages[cell] = battery_status.voltage_cell_v[cell] * 1000.0f;
|
||||
|
||||
} else {
|
||||
bat_msg.voltages[cell] = UINT16_MAX;
|
||||
|
|
Loading…
Reference in New Issue