aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_BattMonitor: check a PEC support using SpecificationInfo

Browse Source
This commit is contained in:
Tatsuya Yamaguchi 2017-02-19 17:50:06 +09:00 committed by Randy Mackay
parent eded4f90bb
commit b052f96f6c
2 changed files with 159 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

147
libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp Normal file → Executable file
View File

@ -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;
}

15
libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.h Normal file → Executable file
View File

@ -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;
};