From 9c3b97347a209e7711008ea93b11242d93a22d1d Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Wed, 19 Apr 2017 13:10:15 -0700 Subject: [PATCH] AP_BattMonitor: Unify read_word interface --- .../AP_BattMonitor/AP_BattMonitor_SMBus.cpp | 28 ++++++++++++++ .../AP_BattMonitor/AP_BattMonitor_SMBus.h | 14 ++++++- .../AP_BattMonitor_SMBus_Maxell.cpp | 37 +------------------ .../AP_BattMonitor_SMBus_Maxell.h | 6 --- .../AP_BattMonitor_SMBus_Solo.cpp | 33 +---------------- .../AP_BattMonitor_SMBus_Solo.h | 5 --- 6 files changed, 43 insertions(+), 80 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp index 7097be4658..49c08850a0 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp @@ -2,6 +2,34 @@ #define AP_BATTMONITOR_SMBUS_PEC_POLYNOME 0x07 // Polynome for CRC generation +// read word from register +// returns true if read was successful, false if failed +bool AP_BattMonitor_SMBus::read_word(uint8_t reg, uint16_t& data) const +{ + // buffer to hold results (1 extra byte returned holding PEC) + const uint8_t read_size = 2 + (_pec_supported ? 1 : 0); + uint8_t buff[read_size]; // buffer to hold results + + // read the appropriate register from the device + if (!_dev->read_registers(reg, buff, sizeof(buff))) { + return false; + } + + // check PEC + if (_pec_supported) { + const 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; +} + /// 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 9a8d424796..f321efbeb1 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h @@ -4,6 +4,7 @@ #include #include #include "AP_BattMonitor_Backend.h" +#include #define AP_BATTMONITOR_SMBUS_BUS_INTERNAL 0 #define AP_BATTMONITOR_SMBUS_BUS_EXTERNAL 1 @@ -15,8 +16,11 @@ class AP_BattMonitor_SMBus : public AP_BattMonitor_Backend public: /// Constructor - AP_BattMonitor_SMBus(AP_BattMonitor &mon, uint8_t instance, AP_BattMonitor::BattMonitor_State &mon_state) : - AP_BattMonitor_Backend(mon, instance, mon_state) + AP_BattMonitor_SMBus(AP_BattMonitor &mon, uint8_t instance, + AP_BattMonitor::BattMonitor_State &mon_state, + AP_HAL::OwnPtr dev) + : AP_BattMonitor_Backend(mon, instance, mon_state), + _dev(std::move(dev)) {} // virtual destructor to reduce compiler warnings @@ -25,10 +29,16 @@ public: protected: + // 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 - 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 _dev; + bool _pec_supported; // true if PEC is supported }; // include specific implementations diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp index a3aed1f750..d7c92bcedb 100755 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp @@ -5,12 +5,6 @@ #include "AP_BattMonitor_SMBus_Maxell.h" #include -extern const AP_HAL::HAL& hal; - -#include - - -#define BATTMONITOR_SMBUS_MAXELL_TEMP 0x08 // temperature register #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 @@ -45,8 +39,7 @@ uint8_t maxell_cell_ids[] = { 0x3f, // cell 1 AP_BattMonitor_SMBus_Maxell::AP_BattMonitor_SMBus_Maxell(AP_BattMonitor &mon, uint8_t instance, AP_BattMonitor::BattMonitor_State &mon_state, AP_HAL::OwnPtr dev) - : AP_BattMonitor_SMBus(mon, instance, mon_state) - , _dev(std::move(dev)) + : AP_BattMonitor_SMBus(mon, instance, mon_state, std::move(dev)) { _dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_SMBus_Maxell::timer, void)); } @@ -102,34 +95,6 @@ void AP_BattMonitor_SMBus_Maxell::timer() } } -// read word from register -// returns true if read was successful, false if failed -bool AP_BattMonitor_SMBus_Maxell::read_word(uint8_t reg, uint16_t& data) const -{ - // buffer to hold results (1 extra byte returned holding PEC) - const uint8_t read_size = 2 + (_pec_supported ? 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_supported) { - const 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 { diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.h index c6c8f2b91d..31e2db10a4 100755 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.h @@ -22,10 +22,6 @@ private: void timer(void); - // read word from register - // returns true if read was successful, false if failed - bool read_word(uint8_t reg, uint16_t& data) const; - // check if PEC supported with the version value in SpecificationInfo() function // returns true once PEC is confirmed as working or not working bool check_pec_support(); @@ -33,7 +29,5 @@ 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; - AP_HAL::OwnPtr _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 }; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp index b2970d98a7..59d5478024 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp @@ -6,11 +6,6 @@ #include "AP_BattMonitor_SMBus_Solo.h" #include -extern const AP_HAL::HAL& hal; - -#include - -#define BATTMONITOR_SMBUS_SOLO_TEMP 0x08 // temperature register #define BATTMONITOR_SMBUS_SOLO_REMAINING_CAPACITY 0x0f // predicted remaining battery capacity in milliamps #define BATTMONITOR_SMBUS_SOLO_FULL_CHARGE_CAPACITY 0x10 // full capacity register #define BATTMONITOR_SMBUS_SOLO_MANUFACTURE_DATA 0x23 /// manufacturer data @@ -37,9 +32,9 @@ extern const AP_HAL::HAL& hal; AP_BattMonitor_SMBus_Solo::AP_BattMonitor_SMBus_Solo(AP_BattMonitor &mon, uint8_t instance, AP_BattMonitor::BattMonitor_State &mon_state, AP_HAL::OwnPtr dev) - : AP_BattMonitor_SMBus(mon, instance, mon_state) - , _dev(std::move(dev)) + : AP_BattMonitor_SMBus(mon, instance, mon_state, std::move(dev)) { + _pec_supported = true; _dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_SMBus_Solo::timer, void)); } @@ -129,30 +124,6 @@ void AP_BattMonitor_SMBus_Solo::timer() } } -// read word from register -// returns true if read was successful, false if failed -bool AP_BattMonitor_SMBus_Solo::read_word(uint8_t reg, uint16_t& data) const -{ - uint8_t buff[3]; // 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 - 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_Solo::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero) const { diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.h index a3485082b4..ce6f4e6e44 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.h @@ -22,13 +22,8 @@ private: void timer(void); - // read word from register - // 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 max_len, bool append_zero) const; - AP_HAL::OwnPtr _dev; uint8_t _button_press_count; };