BattMon_SMBus_I2C: add get_PEC method

libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.cpp

@@ -126,4 +126,45 @@ uint8_t AP_BattMonitor_SMBus_I2C::read_block(uint8_t reg, uint8_t* data, uint8_t
     return bufflen;
 }
 
+#define SMBUS_PEC_POLYNOME  0x07 // Polynome for CRC generation
+
+/// get_PEC - calculate packet error correction code of buffer
+uint8_t AP_BattMonitor_SMBus_I2C::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;
+}
+
 #endif // CONFIG_HAL_BOARD != HAL_BOARD_PX4

libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.h

@@ -25,6 +25,10 @@ private:
 
     // read_block - returns number of characters read if successful, zero if unsuccessful
     uint8_t read_block(uint8_t reg, uint8_t* data, uint8_t max_len, 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;
 };
 
 #endif // AP_BATTMONITOR_SMBUS_I2C_H