diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp index 6d4589d1fa..591c8fe521 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp @@ -11,9 +11,11 @@ #include "AP_BattMonitor_SMBus_Solo.h" #define BATTMONITOR_SMBUS_SOLO_CELL_VOLTAGE 0x28 // cell voltage register +#define BATTMONITOR_SMBUS_SOLO_CELL_VOLTAGE_EXT 0x29 // cell voltage register up to 6s #define BATTMONITOR_SMBUS_SOLO_CURRENT 0x2a // current register #define BATTMONITOR_SMBUS_SOLO_BUTTON_DEBOUNCE 6 // button held down for 5 intervals will cause a power off event #define BATTMONITOR_SMBUS_SOLO_NUM_CELLS 4 // solo's battery pack is 4S +#define BATTMONITOR_SMBUS_SOLO_NUM_CELLS_EXT 6 // extended BMS supports up to 6s /* * Other potentially useful registers, listed here for future use @@ -39,12 +41,12 @@ AP_BattMonitor_SMBus_Solo::AP_BattMonitor_SMBus_Solo(AP_BattMonitor &mon, void AP_BattMonitor_SMBus_Solo::timer() { - uint8_t buff[8]; + uint8_t buff[12]; uint32_t tnow = AP_HAL::micros(); // read cell voltages - if (read_block(BATTMONITOR_SMBUS_SOLO_CELL_VOLTAGE, buff, 8)) { + if (!_use_extended && read_block(BATTMONITOR_SMBUS_SOLO_CELL_VOLTAGE, buff, 8)) { float pack_voltage_mv = 0.0f; for (uint8_t i = 0; i < BATTMONITOR_SMBUS_SOLO_NUM_CELLS; i++) { uint16_t cell = buff[(i * 2) + 1] << 8 | buff[i * 2]; @@ -62,6 +64,25 @@ void AP_BattMonitor_SMBus_Solo::timer() _state.healthy = true; } + + // read extended cell voltages + if (read_block(BATTMONITOR_SMBUS_SOLO_CELL_VOLTAGE_EXT, buff, 12)) { + float pack_voltage_mv = 0.0f; + for (uint8_t i = 0; i < BATTMONITOR_SMBUS_SOLO_NUM_CELLS_EXT; i++) { + uint16_t cell = buff[(i * 2) + 1] << 8 | buff[i * 2]; + _state.cell_voltages.cells[i] = cell; + pack_voltage_mv += (float)cell; + } + _has_cell_voltages = true; + + // accumulate the pack voltage out of the total of the cells + _state.voltage = pack_voltage_mv * 1e-3f; + _state.last_time_micros = tnow; + _state.healthy = true; + // stop reqesting 4-cell packets. + _use_extended = true; + } + // timeout after 5 seconds if ((tnow - _state.last_time_micros) > AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS) { _state.healthy = false; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.h index f14b0e16c0..b9083f5747 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.h @@ -18,6 +18,7 @@ private: void timer(void) override; uint8_t _button_press_count; + bool _use_extended; }; #endif // AP_BATTERY_SMBUS_SOLO_ENABLED