diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp index 0578fae41f..d5da439381 100755 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp @@ -15,6 +15,14 @@ extern const AP_HAL::HAL& hal; #define BATTMONITOR_SMBUS_MAXELL_SPECIFICATION_INFO 0x1a // specification info #define BATTMONITOR_SMBUS_MAXELL_MANUFACTURE_NAME 0x20 // manufacturer name +#define BATTMONITOR_SMBUS_MAXELL_CELL_VOLTAGES 6 +uint8_t maxell_cell_ids[] = { 0x3f, // cell 1 + 0x3e, // cell 2 + 0x3d, // cell 3 + 0x3c, // cell 4 + 0x3b, // cell 5 + 0x3a}; // cell 6 + #define SMBUS_READ_BLOCK_MAXIMUM_TRANSFER 0x20 // A Block Read or Write is allowed to transfer a maximum of 32 data bytes. #define SMBUS_PEC_POLYNOME 0x07 // Polynome for CRC generation @@ -28,12 +36,6 @@ extern const AP_HAL::HAL& hal; * #define BATTMONITOR_SMBUS_MAXELL_DESIGN_VOLTAGE 0x19 // design voltage register * #define BATTMONITOR_SMBUS_MAXELL_MANUFACTURE_DATE 0x1b // manufacturer date * #define BATTMONITOR_SMBUS_MAXELL_SERIALNUM 0x1c // serial number register - * #define BATTMONITOR_SMBUS_MAXELL_CELL_VOLTAGE6 0x3a // cell voltage register - * #define BATTMONITOR_SMBUS_MAXELL_CELL_VOLTAGE5 0x3b // cell voltage register - * #define BATTMONITOR_SMBUS_MAXELL_CELL_VOLTAGE4 0x3c // cell voltage register - * #define BATTMONITOR_SMBUS_MAXELL_CELL_VOLTAGE3 0x3d // cell voltage register - * #define BATTMONITOR_SMBUS_MAXELL_CELL_VOLTAGE2 0x3e // cell voltage register - * #define BATTMONITOR_SMBUS_MAXELL_CELL_VOLTAGE1 0x3f // cell voltage register * #define BATTMONITOR_SMBUS_MAXELL_HEALTH_STATUS 0x4f // state of health * #define BATTMONITOR_SMBUS_MAXELL_SAFETY_ALERT 0x50 // safety alert * #define BATTMONITOR_SMBUS_MAXELL_SAFETY_STATUS 0x50 // safety status @@ -74,6 +76,14 @@ void AP_BattMonitor_SMBus_Maxell::timer() _state.healthy = true; } + for (uint8_t i = 0; i < BATTMONITOR_SMBUS_MAXELL_CELL_VOLTAGES; i++) { + if (read_word(maxell_cell_ids[i], data)) { + _state.cell_voltages.cells[i] = data; + } else { + _state.cell_voltages.cells[i] = UINT16_MAX; + } + } + // timeout after 5 seconds if ((tnow - _state.last_time_micros) > AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS) { _state.healthy = false;