diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index aabada5aa4..54f5283011 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -199,19 +199,29 @@ void GCS_MAVLINK::send_battery_status(const uint8_t instance) const float temp; bool got_temperature = battery.get_temperature(temp, instance); - // ensure we always send a voltage estimate to the GCS, because not all battery monitors monitor individual cells - // as a work around for this we create a set of fake cells to be used if the backend doesn't provide direct monitoring - // the GCS can then recover the pack voltage by summing all non ignored cell values. Because this is looped we can - // report a pack up to 655.34 V with this method - AP_BattMonitor::cells fake_cells; - if (!battery.has_cell_voltages(instance)) { + // prepare array of individual cell voltage + uint16_t cell_volts[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN]; + if (battery.has_cell_voltages(instance)) { + const AP_BattMonitor::cells& batt_cells = battery.get_cell_voltages(instance); + for (uint8_t i = 0; i < ARRAY_SIZE(batt_cells.cells); i++) { + if (i < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN) { + cell_volts[i] = batt_cells.cells[i]; + } else { + // 10th cell reports the lowest voltage of the last 3 cells + cell_volts[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN-1] = MIN(cell_volts[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN-1], batt_cells.cells[i]); + } + } + } else { + // for battery monitors that cannot provide voltages for individual cells the battery's total voltage is put into the first cell + // if the total voltage cannot fit into a single field, the remainder into subsequent fields. + // the GCS can then recover the pack voltage by summing all non ignored cell values an we can report a pack up to 655.34 V float voltage = battery.voltage(instance) * 1e3f; for (uint8_t i = 0; i < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN; i++) { if (voltage < 0.001f) { // too small to send to the GCS, set it to the no cell value - fake_cells.cells[i] = UINT16_MAX; + cell_volts[i] = UINT16_MAX; } else { - fake_cells.cells[i] = MIN(voltage, 65534.0f); // Can't send more then UINT16_MAX - 1 in a cell + cell_volts[i] = MIN(voltage, 65534.0f); // Can't send more then UINT16_MAX - 1 in a cell voltage -= 65534.0f; } } @@ -236,7 +246,7 @@ void GCS_MAVLINK::send_battery_status(const uint8_t instance) const MAV_BATTERY_FUNCTION_UNKNOWN, // function MAV_BATTERY_TYPE_UNKNOWN, // type got_temperature ? ((int16_t) (temp * 100)) : INT16_MAX, // temperature. INT16_MAX if unknown - battery.has_cell_voltages(instance) ? battery.get_cell_voltages(instance).cells : fake_cells.cells, // cell voltages + cell_volts, // cell voltages current, // current in centiampere consumed_mah, // total consumed current in milliampere.hour consumed_wh, // consumed energy in hJ (hecto-Joules)