From cc6298e7acf540031572023fd944a67d43f930e0 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 3 Jun 2020 10:59:54 +0900 Subject: [PATCH] AP_BattMonitor: SMBus Generic reads up to 12 cell voltages Checks for the highest cell for 15 seconds and then reduces the cell voltage calls to cover only those cells that actually exist also renames SMBus _last_cell_update_us --- libraries/AP_BattMonitor/AP_BattMonitor.h | 4 +- .../AP_BattMonitor_SMBus_Generic.cpp | 52 +++++++++++++++---- .../AP_BattMonitor_SMBus_Generic.h | 7 ++- 3 files changed, 49 insertions(+), 14 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index 885b9678c3..54c2150284 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -19,6 +19,8 @@ #define AP_BATT_MONITOR_RES_EST_TC_1 0.5f #define AP_BATT_MONITOR_RES_EST_TC_2 0.1f +#define AP_BATT_MONITOR_CELLS_MAX 12 + #ifndef HAL_BATTMON_SMBUS_ENABLE #define HAL_BATTMON_SMBUS_ENABLE 1 #endif @@ -71,7 +73,7 @@ public: } struct cells { - uint16_t cells[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN]; + uint16_t cells[AP_BATT_MONITOR_CELLS_MAX]; }; // The BattMonitor_State structure is filled in by the backend driver diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Generic.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Generic.cpp index 3aa5059a45..09bfd17267 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Generic.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Generic.cpp @@ -5,14 +5,21 @@ #include "AP_BattMonitor_SMBus_Generic.h" #include -uint8_t maxell_cell_ids[] = { 0x3f, // cell 1 - 0x3e, // cell 2 - 0x3d, // cell 3 - 0x3c, // cell 4 - 0x3b, // cell 5 - 0x3a}; // cell 6 +uint8_t smbus_cell_ids[] = { 0x3f, // cell 1 + 0x3e, // cell 2 + 0x3d, // cell 3 + 0x3c, // cell 4 + 0x3b, // cell 5 + 0x3a, // cell 6 + 0x39, // cell 7 + 0x38, // cell 8 + 0x37, // cell 9 + 0x36, // cell 10 + 0x35, // cell 11 + 0x34}; // cell 12 #define SMBUS_READ_BLOCK_MAXIMUM_TRANSFER 0x20 // A Block Read or Write is allowed to transfer a maximum of 32 data bytes. +#define SMBUS_CELL_COUNT_CHECK_TIMEOUT 15000000 // check cell count for up to 15 seconds /* * Other potentially useful registers, listed here for future use @@ -44,7 +51,7 @@ void AP_BattMonitor_SMBus_Generic::timer() return; } - uint16_t data; + uint16_t data = UINT16_MAX; uint32_t tnow = AP_HAL::micros(); // read voltage (V) @@ -54,13 +61,36 @@ void AP_BattMonitor_SMBus_Generic::timer() _state.healthy = true; } + // assert that BATTMONITOR_SMBUS_NUM_CELLS_MAX matches smbus_cell_ids + static_assert(BATTMONITOR_SMBUS_NUM_CELLS_MAX == ARRAY_SIZE(smbus_cell_ids), "BATTMONITOR_SMBUS_NUM_CELLS_MAX must match smbus_cell_ids"); + + // check cell count + if (!_cell_count_fixed) { + if (_state.healthy) { + // when battery first becomes healthy, start check of cell count + if (_cell_count_check_start_us == 0) { + _cell_count_check_start_us = tnow; + } + if (tnow - _cell_count_check_start_us > SMBUS_CELL_COUNT_CHECK_TIMEOUT) { + // give up checking cell count after 15sec of continuous healthy battery reads + _cell_count_fixed = true; + } + } else { + // if battery becomes unhealthy restart cell count check + _cell_count_check_start_us = 0; + } + } + // read cell voltages - for (uint8_t i = 0; i < BATTMONITOR_SMBUS_MAXELL_NUM_CELLS; i++) { - if (read_word(maxell_cell_ids[i], data)) { + for (uint8_t i = 0; i < (_cell_count_fixed ? _cell_count : BATTMONITOR_SMBUS_NUM_CELLS_MAX); i++) { + if (read_word(smbus_cell_ids[i], data) && (data > 0) && (data < UINT16_MAX)) { _has_cell_voltages = true; _state.cell_voltages.cells[i] = data; - _last_cell_update_ms[i] = tnow; - } else if ((tnow - _last_cell_update_ms[i]) > AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS) { + _last_cell_update_us[i] = tnow; + if (!_cell_count_fixed) { + _cell_count = MAX(_cell_count, i + 1); + } + } else if ((tnow - _last_cell_update_us[i]) > AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS) { _state.cell_voltages.cells[i] = UINT16_MAX; } } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Generic.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Generic.h index be88c38155..003229f746 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Generic.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Generic.h @@ -2,7 +2,7 @@ #include "AP_BattMonitor_SMBus.h" -#define BATTMONITOR_SMBUS_MAXELL_NUM_CELLS 6 +#define BATTMONITOR_SMBUS_NUM_CELLS_MAX 12 class AP_BattMonitor_SMBus_Generic : public AP_BattMonitor_SMBus { @@ -26,5 +26,8 @@ private: uint8_t read_block(uint8_t reg, uint8_t* data, bool append_zero) const; uint8_t _pec_confirmed; // count of the number of times PEC has been confirmed as working - uint32_t _last_cell_update_ms[BATTMONITOR_SMBUS_MAXELL_NUM_CELLS]; // system time of last successful read of cell voltage + uint32_t _last_cell_update_us[BATTMONITOR_SMBUS_NUM_CELLS_MAX]; // system time of last successful read of cell voltage + uint32_t _cell_count_check_start_us; // system time we started attempting to count the number of cells + uint8_t _cell_count; // number of cells returning voltages + bool _cell_count_fixed; // true when cell count check is complete };