Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_BattMonitor: Use a common PEC helper

Browse Source
This commit is contained in:
Michael du Breuil 2017-04-19 12:15:24 -07:00 committed by Francisco Ferreira
parent 35a965ebc5
commit 3319c3ccdc
6 changed files with 50 additions and 89 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp Normal file
View File

@ -0,0 +1,43 @@
#include "AP_BattMonitor_SMBus.h"
#define AP_BATTMONITOR_SMBUS_PEC_POLYNOME 0x07 // Polynome for CRC generation
/// get_PEC - calculate packet error correction code of buffer
uint8_t AP_BattMonitor_SMBus::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 ^= AP_BATTMONITOR_SMBUS_PEC_POLYNOME;
}
}
}
// return result
return crc;
}

7
libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h
View File

@ -22,6 +22,13 @@ public:
// virtual destructor to reduce compiler warnings
virtual ~AP_BattMonitor_SMBus() {}
protected:
// 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;
};
// include specific implementations

40
libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp
View File

@ -26,8 +26,6 @@ uint8_t maxell_cell_ids[] = { 0x3f, // cell 1
#define SMBUS_READ_BLOCK_MAXIMUM_TRANSFER 0x20 // A Block Read or Write is allowed to transfer a maximum of 32 data bytes.
#define SMBUS_PEC_POLYNOME 0x07 // Polynome for CRC generation
/*
* Other potentially useful registers, listed here for future use
* #define BATTMONITOR_SMBUS_MAXELL_CHARGE_STATUS 0x0d // relative state of charge
@ -218,41 +216,3 @@ bool AP_BattMonitor_SMBus_Maxell::check_pec_support()
return true;
}
/// 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;
}

4
libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.h
View File

@ -33,10 +33,6 @@ private:
// 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;
uint8_t _pec_confirmed; // count of the number of times PEC has been confirmed as working
bool _pec_supported; // true if pec is supported

41
libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp
View File

@ -189,44 +189,3 @@ uint8_t AP_BattMonitor_SMBus_Solo::read_block(uint8_t reg, uint8_t* data, uint8_
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_Solo::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;
}

4
libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.h
View File

@ -29,10 +29,6 @@ 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;
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
uint8_t _button_press_count;
};