diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp index ffaae0b9f1..1eb3919cf6 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp @@ -173,6 +173,45 @@ bool AP_BattMonitor_SMBus::read_word(uint8_t reg, uint16_t& data) const return true; } +// read_block - returns number of characters read if successful, zero if unsuccessful +uint8_t AP_BattMonitor_SMBus::read_block(uint8_t reg, uint8_t* data, uint8_t len) 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 > len) { + return 0; + } + + // buffer to hold results (2 extra byte returned holding length and PEC) + const uint8_t read_size = bufflen + 1 + (_pec_supported ? 1 : 0); + uint8_t buff[read_size]; + + // read bytes + if (!_dev->read_registers(reg, buff, read_size)) { + return 0; + } + + // check PEC + if (_pec_supported) { + const uint8_t pec = get_PEC(_address, reg, true, buff, bufflen+1); + if (pec != buff[bufflen+1]) { + return 0; + } + } + + // copy data (excluding length & PEC) + memcpy(data, &buff[1], bufflen); + + // return success + return bufflen; +} + /// 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 { diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h index 3c26aba47e..7b5330f91a 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h @@ -10,7 +10,8 @@ #define AP_BATTMONITOR_SMBUS_BUS_INTERNAL 0 #define AP_BATTMONITOR_SMBUS_BUS_EXTERNAL 1 #define AP_BATTMONITOR_SMBUS_I2C_ADDR 0x0B -#define AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS 5000000 // sensor becomes unhealthy if no successful readings for 5 seconds +#define AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS 5000000 // sensor becomes unhealthy if no successful readings for 5 seconds +#define AP_BATTMONITOR_SMBUS_READ_BLOCK_MAXIMUM_TRANSFER 0x20 // A Block Read or Write is allowed to transfer a maximum of 32 data bytes. class AP_BattMonitor_SMBus : public AP_BattMonitor_Backend { @@ -84,6 +85,9 @@ protected: // returns true if read was successful, false if failed bool read_word(uint8_t reg, uint16_t& data) const; + // read_block - returns number of characters read if successful, zero if unsuccessful + uint8_t read_block(uint8_t reg, uint8_t* data, uint8_t len) 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; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Generic.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Generic.cpp index 83d33c8984..9d02f30888 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Generic.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Generic.cpp @@ -3,7 +3,6 @@ #include #include "AP_BattMonitor.h" #include "AP_BattMonitor_SMBus_Generic.h" -#include uint8_t smbus_cell_ids[] = { 0x3f, // cell 1 0x3e, // cell 2 @@ -23,7 +22,6 @@ uint8_t smbus_cell_ids[] = { 0x3f, // cell 1 #endif }; -#define SMBUS_READ_BLOCK_MAXIMUM_TRANSFER 0x20 // A Block Read or Write is allowed to transfer a maximum of 32 data bytes. #define SMBUS_CELL_COUNT_CHECK_TIMEOUT 15 // check cell count for up to 15 seconds /* @@ -129,45 +127,6 @@ void AP_BattMonitor_SMBus_Generic::timer() read_cycle_count(); } -// read_block - returns number of characters read if successful, zero if unsuccessful -uint8_t AP_BattMonitor_SMBus_Generic::read_block(uint8_t reg, uint8_t* data) 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 > SMBUS_READ_BLOCK_MAXIMUM_TRANSFER) { - return 0; - } - - // buffer to hold results (2 extra byte returned holding length and PEC) - const uint8_t read_size = bufflen + 1 + (_pec_supported ? 1 : 0); - uint8_t buff[read_size]; - - // read bytes - if (!_dev->read_registers(reg, buff, read_size)) { - return 0; - } - - // check PEC - if (_pec_supported) { - 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); - - // return success - return bufflen; -} - // check if PEC supported with the version value in SpecificationInfo() function // returns true once PEC is confirmed as working or not working bool AP_BattMonitor_SMBus_Generic::check_pec_support() @@ -194,8 +153,8 @@ bool AP_BattMonitor_SMBus_Generic::check_pec_support() } // check manufacturer name - uint8_t buff[SMBUS_READ_BLOCK_MAXIMUM_TRANSFER + 1] {}; - if (read_block(BATTMONITOR_SMBUS_MANUFACTURE_NAME, buff)) { + uint8_t buff[AP_BATTMONITOR_SMBUS_READ_BLOCK_MAXIMUM_TRANSFER + 1] {}; + if (read_block(BATTMONITOR_SMBUS_MANUFACTURE_NAME, buff, sizeof(buff))) { // Hitachi maxell batteries do not support PEC if (strcmp((char*)buff, "Hitachi maxell") == 0) { _pec_supported = false; @@ -209,4 +168,3 @@ bool AP_BattMonitor_SMBus_Generic::check_pec_support() _pec_confirmed = true; return true; } - diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Generic.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Generic.h index fd19a830b2..7edc4a6264 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Generic.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Generic.h @@ -25,9 +25,6 @@ private: // returns true once PEC is confirmed as working or not working bool check_pec_support(); - // read_block - returns number of characters read if successful, zero if unsuccessful - uint8_t read_block(uint8_t reg, uint8_t* data) const; - uint8_t _pec_confirmed; // count of the number of times PEC has been confirmed as working uint32_t _last_cell_update_us[BATTMONITOR_SMBUS_NUM_CELLS_MAX]; // system time of last successful read of cell voltage uint32_t _cell_count_check_start_us; // system time we started attempting to count the number of cells diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_SUI.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_SUI.cpp index b1382f7d9e..7d8aacce33 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_SUI.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_SUI.cpp @@ -65,38 +65,6 @@ void AP_BattMonitor_SMBus_SUI::timer() update_health(); } -// read_block - returns true if successful -bool AP_BattMonitor_SMBus_SUI::read_block(uint8_t reg, uint8_t* data, uint8_t len) const -{ - // buffer to hold results (2 extra byte returned holding length and PEC) - uint8_t buff[len+2]; - - // read bytes - if (!_dev->read_registers(reg, buff, sizeof(buff))) { - return false; - } - - // get length - uint8_t bufflen = buff[0]; - - // sanity check length returned by smbus - if (bufflen == 0 || bufflen > len) { - return false; - } - - // check PEC - uint8_t pec = get_PEC(_address, reg, true, buff, bufflen+1); - if (pec != buff[bufflen+1]) { - return false; - } - - // copy data (excluding PEC) - memcpy(data, &buff[1], bufflen); - - // return success - return true; -} - // read_bare_block - returns true if successful bool AP_BattMonitor_SMBus_SUI::read_block_bare(uint8_t reg, uint8_t* data, uint8_t len) const { @@ -129,7 +97,7 @@ void AP_BattMonitor_SMBus_SUI::read_cell_voltages() // we can't read voltage of all cells. get overall pack voltage to work out // an average for remaining cells uint16_t total_mv; - if (read_block(BATTMONITOR_SMBUS_VOLTAGE, (uint8_t *)&total_mv, sizeof(total_mv))) { + if (read_word(BATTMONITOR_SMBUS_VOLTAGE, total_mv)) { // if total voltage is below pack_voltage_mv then we will // read zero volts for the extra cells. total_mv = MAX(total_mv, pack_voltage_mv); diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_SUI.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_SUI.h index 5ba8ad913e..506f5bc541 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_SUI.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_SUI.h @@ -21,8 +21,7 @@ private: void read_cell_voltages(); void update_health(); - // read_block - returns number of characters read if successful, zero if unsuccessful - bool read_block(uint8_t reg, uint8_t* data, uint8_t len) const; + // read_block_bare - returns number of characters read if successful, zero if unsuccessful bool read_block_bare(uint8_t reg, uint8_t* data, uint8_t len) const; const uint8_t cell_count; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp index 0528d70ca6..5d4be88342 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp @@ -95,34 +95,3 @@ void AP_BattMonitor_SMBus_Solo::timer() read_cycle_count(); } - -// read_block - returns number of characters read if successful, zero if unsuccessful -uint8_t AP_BattMonitor_SMBus_Solo::read_block(uint8_t reg, uint8_t* data, uint8_t max_len) const -{ - uint8_t buff[max_len+2]; // buffer to hold results (2 extra byte returned holding length and PEC) - - // read bytes - if (!_dev->read_registers(reg, buff, sizeof(buff))) { - return 0; - } - - // get length - uint8_t bufflen = buff[0]; - - // sanity check length returned by smbus - if (bufflen == 0 || bufflen > max_len) { - return 0; - } - - // check PEC - uint8_t pec = get_PEC(_address, reg, true, buff, bufflen+1); - if (pec != buff[bufflen+1]) { - return 0; - } - - // copy data (excluding PEC) - memcpy(data, &buff[1], bufflen); - - // return success - return bufflen; -} diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.h index d343f3f7bd..2200220b46 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.h @@ -15,8 +15,5 @@ private: void timer(void) override; - // 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) const; - uint8_t _button_press_count; };