From bf0e5a350cf143cb2cdc5228569237a6e8320ffc Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 30 Dec 2014 11:10:01 +0900 Subject: [PATCH] BattMon_SMBus_I2C: add get_PEC method --- .../AP_BattMonitor_SMBus_I2C.cpp | 41 +++++++++++++++++++ .../AP_BattMonitor/AP_BattMonitor_SMBus_I2C.h | 4 ++ 2 files changed, 45 insertions(+) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.cpp index 36dd8437e0..be55ebabae 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.cpp +++ b/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