From a92161cd1823fa3e06ad3e882b16199334e16020 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 25 May 2022 11:31:11 +1000 Subject: [PATCH] GCS_MAVLink: improved start battery cell monitoring this fixes two issues: 1) we are not reporting the sag corrected voltage to GCS when we are sending individual cells 2) we don't cope with having more than AP_BATT_MONITOR_CELLS_MAX cells (or 12 for low flash boards, 14 for 2M boards) it fixes this by distributing the extra voltage over the calls. This change is particularly important for high cell count DroneCAN smart batteries, where we currently would not handle more than 14 cells and the GCS would display the wrong voltage the PR also cleans up the use of volts vs mVolts for the local variables --- libraries/GCS_MAVLink/GCS_Common.cpp | 65 ++++++++++++++++++++++------ 1 file changed, 52 insertions(+), 13 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 3d69f782bd..525c5c34af 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -240,36 +240,75 @@ void GCS_MAVLINK::send_battery_status(const uint8_t instance) const bool got_temperature = battery.get_temperature(temp, instance); // prepare arrays of individual cell voltages - uint16_t cell_volts[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN]; - uint16_t cell_volts_ext[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN]; + uint16_t cell_mvolts[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN]; + uint16_t cell_mvolts_ext[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN]; + const uint16_t max_cell_mV = 0xFFFEU; + const uint16_t invalid_cell_mV = 0xFFFFU; + if (battery.has_cell_voltages(instance)) { const AP_BattMonitor::cells& batt_cells = battery.get_cell_voltages(instance); + static_assert(sizeof(cell_mvolts) <= sizeof(batt_cells.cells), "cell array length not large enough"); + // copy the first 10 cells - memcpy(cell_volts, batt_cells.cells, sizeof(cell_volts)); + memcpy(cell_mvolts, batt_cells.cells, sizeof(cell_mvolts)); // 11 ... 14 use a second cell_volts_ext array for (uint8_t i = 0; i < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN; i++) { if (MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN+i < uint8_t(ARRAY_SIZE(batt_cells.cells))) { - cell_volts_ext[i] = batt_cells.cells[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN+i]; + cell_mvolts_ext[i] = batt_cells.cells[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN+i]; } else { - cell_volts_ext[i] = 0; + cell_mvolts_ext[i] = 0; + } + } + /* + now adjust voltages to cope with two things: + 1) we may be reporting sag corrected voltage + 2) the battery may have more cells than can be reported by the backend, so the actual voltage may be higher than the sum + */ + const float voltage_mV = battery.gcs_voltage(instance) * 1e3f; + float voltage_mV_sum = 0; + uint8_t non_zero_cell_count = 0; + for (uint8_t i=0; i 0 && cell_mvolts[i] != invalid_cell_mV) { + non_zero_cell_count++; + voltage_mV_sum += cell_mvolts[i]; + } + } + for (uint8_t i=0; i 0 && cell_mvolts_ext[i] != invalid_cell_mV) { + non_zero_cell_count++; + voltage_mV_sum += cell_mvolts_ext[i]; + } + } + if (voltage_mV > voltage_mV_sum && non_zero_cell_count > 0) { + // distribute the extra voltage over the non-zero cells + uint32_t extra_mV = (voltage_mV - voltage_mV_sum) / non_zero_cell_count; + for (uint8_t i=0; i 0 && cell_mvolts[i] != invalid_cell_mV) { + cell_mvolts[i] = MIN(cell_mvolts[i] + extra_mV, max_cell_mV); + } + } + for (uint8_t i=0; i 0 && cell_mvolts_ext[i] != invalid_cell_mV) { + cell_mvolts_ext[i] = MIN(cell_mvolts_ext[i] + extra_mV, max_cell_mV); + } } } } 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.gcs_voltage(instance) * 1e3f; + float voltage_mV = battery.gcs_voltage(instance) * 1e3f; for (uint8_t i = 0; i < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN; i++) { - if (voltage < 0.001f) { + if (voltage_mV < 0.001f) { // too small to send to the GCS, set it to the no cell value - cell_volts[i] = UINT16_MAX; + cell_mvolts[i] = UINT16_MAX; } else { - cell_volts[i] = MIN(voltage, 65534.0f); // Can't send more then UINT16_MAX - 1 in a cell - voltage -= 65534.0f; + cell_mvolts[i] = MIN(voltage_mV, max_cell_mV); // Can't send more then UINT16_MAX - 1 in a cell + voltage_mV -= max_cell_mV; } } for (uint8_t i = 0; i < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN; i++) { - cell_volts_ext[i] = 0; + cell_mvolts_ext[i] = 0; } } @@ -299,14 +338,14 @@ 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 - cell_volts, // cell voltages + cell_mvolts, // cell voltages current, // current in centiampere consumed_mah, // total consumed current in milliampere.hour consumed_wh, // consumed energy in hJ (hecto-Joules) percentage, time_remaining, // time remaining, seconds battery.get_mavlink_charge_state(instance), // battery charge state - cell_volts_ext, // Cell 11..14 voltages + cell_mvolts_ext, // Cell 11..14 voltages 0, // battery mode battery.get_mavlink_fault_bitmask(instance)); // fault_bitmask #else