diff options
author | Randy Mackay <rmackay9@yahoo.com> | 2014-12-15 16:42:29 +0900 |
---|---|---|
committer | Randy Mackay <rmackay9@yahoo.com> | 2014-12-30 13:43:21 +0900 |
commit | a952a18b42d486e6f3ba1d52506fbd2ba2fc3425 (patch) | |
tree | e64232223da866d84378d77ced6bb4ac042e6df2 /src/drivers/batt_smbus/batt_smbus.cpp | |
parent | 65bcd0e1226f5f5d6db28dafe5faea771ccbde1d (diff) | |
download | px4-firmware-a952a18b42d486e6f3ba1d52506fbd2ba2fc3425.tar.gz px4-firmware-a952a18b42d486e6f3ba1d52506fbd2ba2fc3425.tar.bz2 px4-firmware-a952a18b42d486e6f3ba1d52506fbd2ba2fc3425.zip |
batt_smbus: add get_PEC
Diffstat (limited to 'src/drivers/batt_smbus/batt_smbus.cpp')
-rw-r--r-- | src/drivers/batt_smbus/batt_smbus.cpp | 84 |
1 files changed, 77 insertions, 7 deletions
diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 882b38f7e..4d4a84049 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -84,10 +84,13 @@ #define BATT_SMBUS_DESIGN_CAPACITY 0x18 /* design capacity register */ #define BATT_SMBUS_DESIGN_VOLTAGE 0x19 /* design voltage register */ #define BATT_SMBUS_SERIALNUM 0x1c /* serial number register */ +#define BATT_SMBUS_MANUFACTURE_NAME 0x20 /* manufacturer name */ #define BATT_SMBUS_MANUFACTURE_INFO 0x25 /* cell voltage register */ #define BATT_SMBUS_CURRENT 0x2a /* current register */ #define BATT_SMBUS_MEASUREMENT_INTERVAL_MS (1000000 / 10) /* time in microseconds, measure at 10hz */ +#define BATT_SMBUS_PEC_POLYNOMIAL 0x07 /* Polynomial for calculating PEC */ + #ifndef CONFIG_SCHED_WORKQUEUE # error This requires CONFIG_SCHED_WORKQUEUE. #endif @@ -124,6 +127,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); + // 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(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const; + // internal variables work_s _work; // work queue for scheduling reads RingBuffer *_reports; // buffer of recorded voltages, currents @@ -272,7 +279,12 @@ BATT_SMBUS::cycle() // convert millivolts to volts new_report.voltage_v = ((float)tmp) / 1000.0f; - // To-Do: read current as block from BATT_SMBUS_CURRENT register + // read current + usleep(1); + uint8_t buff[4]; + if (read_block(BATT_SMBUS_CURRENT, buff, 4, false) == 4) { + new_report.current_a = (float)((int32_t)((uint32_t)buff[3]<<24 | (uint32_t)buff[2]<<16 | (uint32_t)buff[1]<<8 | (uint32_t)buff[0])) / 1000.0f; + } // publish to orb if (_batt_topic != -1) { @@ -301,12 +313,20 @@ BATT_SMBUS::cycle() int BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) { - uint8_t buff[2]; + uint8_t buff[3]; // 2 bytes of data + PEC // read from register - int ret = transfer(®, 1, buff, 2); + int ret = transfer(®, 1, buff, 3); if (ret == OK) { - val = (uint16_t)buff[1] << 8 | (uint16_t)buff[0]; + // check PEC + uint8_t pec = get_PEC(reg, true, buff, 2); + if (pec == buff[2]) { + val = (uint16_t)buff[1] << 8 | (uint16_t)buff[0]; + warnx("PEC ok: %x",(int)pec); + } else { + ret = ENOTTY; + warnx("PEC:%x vs MyPec:%x",(int)buff[2],(int)pec); + } } // return success or failure @@ -316,12 +336,12 @@ BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) // read_block - returns number of characters read if successful, zero if unsuccessful uint8_t BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero) { - uint8_t buff[max_len+1]; // buffer to hold results + uint8_t buff[max_len+2]; // buffer to hold results usleep(1); - // read bytes - int ret = transfer(®, 1,buff, max_len+1); + // read bytes including PEC + int ret = transfer(®, 1, buff, max_len+2); // return zero on failure if (ret != OK) { @@ -336,6 +356,16 @@ uint8_t BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool return 0; } + // check PEC + uint8_t pec = get_PEC(reg, true, buff, bufflen+1); + if (pec != buff[bufflen+1]) { + // debug + warnx("CurrPEC:%x vs MyPec:%x",(int)buff[bufflen+1],(int)pec); + return 0; + } else { + warnx("CurPEC ok: %x",(int)pec); + } + // copy data memcpy(data, &buff[1], bufflen); @@ -348,6 +378,46 @@ uint8_t BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool return bufflen; } +// 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 BATT_SMBUS::get_PEC(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] = (uint8_t)get_address() << 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 ^= BATT_SMBUS_PEC_POLYNOMIAL; + } + } + } + + // return result + return crc; +} + ///////////////////////// shell functions /////////////////////// void |