Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: improved start battery cell monitoring

Browse Source 
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
This commit is contained in:
Andrew Tridgell 2022-05-25 11:31:11 +10:00
parent 489a27ca19
commit a92161cd18
1 changed files with 52 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -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<MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN; i++) {
if (cell_mvolts[i] > 0 && cell_mvolts[i] != invalid_cell_mV) {
non_zero_cell_count++;
voltage_mV_sum += cell_mvolts[i];
}
}
for (uint8_t i=0; i<MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN; i++) {
if (cell_mvolts_ext[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<MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN; i++) {
if (cell_mvolts[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<MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN; i++) {
if (cell_mvolts_ext[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