AP_BattMonitor: check a PEC support using SpecificationInfo

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<sizeof(tmp_buff); i++) {
+        // load next data byte into the shift register
+        shift_reg = tmp_buff[i];
+        // for each bit in the current byte
+        for (uint8_t j=0; j<8; j++) {
+            do_invert = (crc ^ shift_reg) & 0x80;
+            crc <<= 1;
+            shift_reg <<= 1;
+            if(do_invert) {
+                crc ^= SMBUS_PEC_POLYNOME;
+            }
+        }
+    }
+
+    // return result
+    return crc;
+}

libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.h

@@ -22,9 +22,24 @@ private:
 
     void timer(void);
 
+    bool _pec_support = false;
+
+    // it sets true after check if PEC is used
+    bool _pec_confirmed = false;
+
     // read word from register
     // returns true if read was successful, false if failed
     bool read_word(uint8_t reg, uint16_t& data) const;
 
+    // get PEC support with the version value in SpecificationInfo() function
+    bool get_pec_support();
+
+    // read_block - returns number of characters read if successful, zero if unsuccessful
+    uint8_t read_block(uint8_t reg, uint8_t* data, bool append_zero) const;
+
+    // get_PEC - calculate PEC for a read or write from the battery
+    //  buff is the data that was read or will be written
+    uint8_t get_PEC(const uint8_t i2c_addr, uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const;
+
     AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
 };