diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp old mode 100644 new mode 100755 index 285c3400be..d09706ab7a --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp @@ -12,6 +12,15 @@ extern const AP_HAL::HAL& hal; #define BATTMONITOR_SMBUS_MAXELL_VOLTAGE 0x09 // voltage register #define BATTMONITOR_SMBUS_MAXELL_CURRENT 0x0a // current register +#define BATTMONITOR_SMBUS_MAXELL_SPECIFICATION_INFO 0x1a // specification info +#define BATTMONITOR_SMBUS_MAXELL_MANUFACTURE_NAME 0x20 // manufacturer name + +#define BATTMONITOR_SMBUS_10_PEC_SUPPORT 0x10 // Smart Battery Specification v1.0 +#define BATTMONITOR_SMBUS_11_PEC_NOT_SUPPORT 0x21 // Smart Battery Specification v1.1 without PEC support +#define BATTMONITOR_SMBUS_11_PEC_SUPPORT 0x31 // Smart Battery Specification v1.1 with PEC support + +// A Block Read or Write is allowed to transfer a maximum of 32 data bytes. +#define READ_BLOCK_MAXIMUM_TRANSFER 0x20 /* * Other potentially useful registers, listed here for future use @@ -20,8 +29,7 @@ extern const AP_HAL::HAL& hal; * #define BATTMONITOR_SMBUS_MAXELL_BATTERY_STATUS 0x16 // battery status register including alarms * #define BATTMONITOR_SMBUS_MAXELL_BATTERY_CYCLE_COUNT 0x17 // cycle count * #define BATTMONITOR_SMBUS_MAXELL_DESIGN_VOLTAGE 0x19 // design voltage register - * #define BATTMONITOR_SMBUS_MAXELL_SPECIFICATION_INFO 0x1a // specification info - * #define BATTMONITOR_SMBUS_MAXELL_MANUFACTURE_NAME 0x1b // manufacturer name + * #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 @@ -54,6 +62,10 @@ void AP_BattMonitor_SMBus_Maxell::read() void AP_BattMonitor_SMBus_Maxell::timer() { + if (!_pec_confirmed) { + get_pec_support(); + } + uint16_t data; uint32_t tnow = AP_HAL::micros(); @@ -81,16 +93,145 @@ void AP_BattMonitor_SMBus_Maxell::timer() // returns true if read was successful, false if failed bool AP_BattMonitor_SMBus_Maxell::read_word(uint8_t reg, uint16_t& data) const { - uint8_t buff[2]; // buffer to hold results + // buffer to hold results (1 extra byte returned holding PEC) + uint8_t read_size = 2 + (_pec_support ? 1 : 0); + uint8_t buff[read_size]; // buffer to hold results // read three bytes and place in last three bytes of buffer if (!_dev->read_registers(reg, buff, sizeof(buff))) { return false; } + // check PEC + if (_pec_support) { + uint8_t pec = get_PEC(AP_BATTMONITOR_SMBUS_I2C_ADDR, reg, true, buff, 2); + if (pec != buff[2]) { + return false; + } + } + // convert buffer to word data = (uint16_t)buff[1]<<8 | (uint16_t)buff[0]; // return success return true; } + +// read_block - returns number of characters read if successful, zero if unsuccessful +uint8_t AP_BattMonitor_SMBus_Maxell::read_block(uint8_t reg, uint8_t* data, bool append_zero) const +{ + // get length + uint8_t bufflen; + // read byte (first byte indicates the number of bytes in the block) + if (!_dev->read_registers(reg, &bufflen, 1)) { + return 0; + } + + // sanity check length returned by smbus + if (bufflen == 0 || bufflen > READ_BLOCK_MAXIMUM_TRANSFER) { + return 0; + } + + // buffer to hold results (2 extra byte returned holding length and PEC) + uint8_t read_size = bufflen + 1 + (_pec_support ? 1 : 0); + uint8_t buff[read_size]; + + // read bytes + if (!_dev->read_registers(reg, buff, read_size)) { + return 0; + } + + // check PEC + if (_pec_support) { + uint8_t pec = get_PEC(AP_BATTMONITOR_SMBUS_I2C_ADDR, reg, true, buff, bufflen+1); + if (pec != buff[bufflen+1]) { + return 0; + } + } + + // copy data (excluding PEC) + memcpy(data, &buff[1], bufflen); + + // optionally add zero to end + if (append_zero) { + data[bufflen] = '\0'; + } + + // return success + return bufflen; +} + +// get PEC support with the version value in SpecificationInfo +bool AP_BattMonitor_SMBus_Maxell::get_pec_support() +{ + uint16_t data; + uint8_t buff[READ_BLOCK_MAXIMUM_TRANSFER + 1]; + + // specification info + if (read_word(BATTMONITOR_SMBUS_MAXELL_SPECIFICATION_INFO, data)) { + if ((data & 0xFF) == BATTMONITOR_SMBUS_11_PEC_SUPPORT) { + if (read_block(BATTMONITOR_SMBUS_MAXELL_MANUFACTURE_NAME, buff, true)) { + // In Hitachi maxell battery, specification info is 0x31 (SBSv1.1 with PEC support) but PEC isn't support + if (strcmp((char*)buff, "Hitachi maxell") == 0) { + _pec_confirmed = true; + _pec_support = false; + return 1; + } + } + _pec_support = true; + // Check if it can get the correct value with PEC support + if (read_word(BATTMONITOR_SMBUS_MAXELL_SPECIFICATION_INFO, data)) { + if ((data & 0xFF) == BATTMONITOR_SMBUS_11_PEC_SUPPORT) { + _pec_confirmed = true; + return 1; + } + } + } else if (((data & 0xFF) == BATTMONITOR_SMBUS_10_PEC_SUPPORT) || ((data & 0xFF) == BATTMONITOR_SMBUS_11_PEC_NOT_SUPPORT)) { + _pec_confirmed = true; + _pec_support = false; + return 1; + } + } + return 0; +} + +#define SMBUS_PEC_POLYNOME 0x07 // Polynome for CRC generation + +/// get_PEC - calculate packet error correction code of buffer +uint8_t AP_BattMonitor_SMBus_Maxell::get_PEC(const uint8_t i2c_addr, uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const +{ + // exit immediately if no data + if (len <= 0) { + return 0; + } + + // prepare temp buffer for calcing crc + uint8_t tmp_buff[len+3]; + tmp_buff[0] = i2c_addr << 1; + tmp_buff[1] = cmd; + tmp_buff[2] = tmp_buff[0] | (uint8_t)reading; + memcpy(&tmp_buff[3],buff,len); + + // initialise crc to zero + uint8_t crc = 0; + uint8_t shift_reg = 0; + bool do_invert; + + // for each byte in the stream + for (uint8_t i=0; i _dev; };