diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 281071a2ef..b0baf49c01 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -170,14 +170,16 @@ AP_BattMonitor::init() drivers[instance] = new AP_BattMonitor_Analog(*this, instance, state[instance]); _num_instances++; break; - case BattMonitor_TYPE_SMBUS: + case BattMonitor_TYPE_SOLO: state[instance].instance = instance; #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 drivers[instance] = new AP_BattMonitor_SMBus_PX4(*this, instance, state[instance]); #else - drivers[instance] = new AP_BattMonitor_SMBus_I2C(*this, instance, state[instance], - hal.i2c_mgr->get_device(BATTMONITOR_SBUS_I2C_BUS, BATTMONITOR_SMBUS_I2C_ADDR)); + drivers[instance] = new AP_BattMonitor_SMBus_Solo(*this, instance, state[instance], #endif + hal.i2c_mgr->get_device(AP_BATTMONITOR_SMBUS_BUS_INTERNAL, AP_BATTMONITOR_SMBUS_I2C_ADDR)); + _num_instances++; + break; _num_instances++; break; case BattMonitor_TYPE_BEBOP: @@ -222,8 +224,8 @@ bool AP_BattMonitor::has_current(uint8_t instance) const // check for analog voltage and current monitor or smbus monitor if (instance < _num_instances && drivers[instance] != nullptr) { return (_monitoring[instance] == BattMonitor_TYPE_ANALOG_VOLTAGE_AND_CURRENT || - _monitoring[instance] == BattMonitor_TYPE_SMBUS || _monitoring[instance] == BattMonitor_TYPE_BEBOP); + _monitoring[instance] == BattMonitor_TYPE_SOLO || } // not monitoring current diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index 72b813e499..07b9e1ab8b 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -18,7 +18,7 @@ class AP_BattMonitor_Backend; class AP_BattMonitor_Analog; class AP_BattMonitor_SMBus; -class AP_BattMonitor_SMBus_I2C; +class AP_BattMonitor_SMBus_Solo; class AP_BattMonitor_SMBus_PX4; class AP_BattMonitor @@ -26,7 +26,7 @@ class AP_BattMonitor friend class AP_BattMonitor_Backend; friend class AP_BattMonitor_Analog; friend class AP_BattMonitor_SMBus; - friend class AP_BattMonitor_SMBus_I2C; + friend class AP_BattMonitor_SMBus_Solo; friend class AP_BattMonitor_SMBus_PX4; public: @@ -39,8 +39,8 @@ public: BattMonitor_TYPE_NONE = 0, BattMonitor_TYPE_ANALOG_VOLTAGE_ONLY = 3, BattMonitor_TYPE_ANALOG_VOLTAGE_AND_CURRENT = 4, - BattMonitor_TYPE_SMBUS = 5, BattMonitor_TYPE_BEBOP = 6 + BattMonitor_TYPE_SOLO = 5, }; // The BattMonitor_State structure is filled in by the backend driver diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h index b522ed075c..349ba1c8b3 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h @@ -5,7 +5,10 @@ #include #include "AP_BattMonitor_Backend.h" -#define AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS 5000000 // sensor becomes unhealthy if no successful readings for 5 seconds +#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 class AP_BattMonitor_SMBus : public AP_BattMonitor_Backend { @@ -23,4 +26,4 @@ public: // include specific implementations #include "AP_BattMonitor_SMBus_PX4.h" -#include "AP_BattMonitor_SMBus_I2C.h" +#include "AP_BattMonitor_SMBus_Solo.h" diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.cpp deleted file mode 100644 index 25e57f0c2f..0000000000 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.cpp +++ /dev/null @@ -1,167 +0,0 @@ -#include -#include -#include -#include "AP_BattMonitor.h" -#include "AP_BattMonitor_SMBus_I2C.h" -#include - -extern const AP_HAL::HAL& hal; - -#include - -#define BATTMONITOR_SMBUS_TEMP 0x08 // temperature register -#define BATTMONITOR_SMBUS_VOLTAGE 0x09 // voltage register -#define BATTMONITOR_SMBUS_FULL_CHARGE_CAPACITY 0x10 // full capacity register -#define BATTMONITOR_SMBUS_BATTERY_STATUS 0x16 // battery status register including alarms -#define BATTMONITOR_SMBUS_DESIGN_CAPACITY 0x18 // design capacity register -#define BATTMONITOR_SMBUS_DESIGN_VOLTAGE 0x19 // design voltage register -#define BATTMONITOR_SMBUS_SERIALNUM 0x1c // serial number register -#define BATTMONITOR_SMBUS_MANUFACTURE_NAME 0x20 // manufacturer name -#define BATTMONITOR_SMBUS_DEVICE_NAME 0x21 // device name -#define BATTMONITOR_SMBUS_DEVICE_CHEMISTRY 0x22 // device chemistry -#define BATTMONITOR_SMBUS_MANUFACTURE_INFO 0x25 // manufacturer info including cell voltage -#define BATTMONITOR_SMBUS_CELL_VOLTAGE 0x28 // cell voltage register -#define BATTMONITOR_SMBUS_CURRENT 0x2a // current register - -// Constructor -AP_BattMonitor_SMBus_I2C::AP_BattMonitor_SMBus_I2C(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)) -{ - _dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_SMBus_I2C::timer, void)); -} - -/// Read the battery voltage and current. Should be called at 10hz -void AP_BattMonitor_SMBus_I2C::read() -{ - // nothing to do - all done in timer() -} - -void AP_BattMonitor_SMBus_I2C::timer() -{ - uint16_t data; - uint8_t buff[4]; - uint32_t tnow = AP_HAL::micros(); - - // read voltage - if (read_word(BATTMONITOR_SMBUS_VOLTAGE, data)) { - _state.voltage = (float)data / 1000.0f; - _state.last_time_micros = tnow; - _state.healthy = true; - } - - // read current - if (read_block(BATTMONITOR_SMBUS_CURRENT, buff, 4, false) == 4) { - _state.current_amps = (float)((int32_t)((uint32_t)buff[3]<<24 | (uint32_t)buff[2]<<16 | (uint32_t)buff[1]<<8 | (uint32_t)buff[0])) / 1000.0f; - _state.last_time_micros = tnow; - } - - // timeout after 5 seconds - if ((tnow - _state.last_time_micros) > AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS) { - _state.healthy = false; - } -} - -// read word from register -// returns true if read was successful, false if failed -bool AP_BattMonitor_SMBus_I2C::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(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_I2C::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero) 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(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; -} - -#define SMBUS_PEC_POLYNOME 0x07 // Polynome for CRC generation - -/// get_PEC - calculate packet error correction code of buffer -uint8_t AP_BattMonitor_SMBus_I2C::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 +#include +#include +#include +#include "AP_BattMonitor.h" +#include "AP_BattMonitor_SMBus_Solo.h" +#include + +extern const AP_HAL::HAL& hal; + +#include + +#define BATTMONITOR_SMBUS_SOLO_VOLTAGE 0x09 // voltage 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 +#define BATTMONITOR_SMBUS_SOLO_CURRENT 0x2a // current register +#define BATTMONITOR_SMBUS_SOLO_BUTTON_DEBOUNCE 3 // button held down for 3 intervals will cause a power off event + +/* + * Other potentially useful registers, listed here for future use + * #define BATTMONITOR_SMBUS_SOLO_TEMP 0x08 // temperature register + * #define BATTMONITOR_SMBUS_SOLO_BATTERY_STATUS 0x16 // battery status register including alarms + * #define BATTMONITOR_SMBUS_SOLO_DESIGN_CAPACITY 0x18 // design capacity register + * #define BATTMONITOR_SMBUS_SOLO_DESIGN_VOLTAGE 0x19 // design voltage register + * #define BATTMONITOR_SMBUS_SOLO_SERIALNUM 0x1c // serial number register + * #define BATTMONITOR_SMBUS_SOLO_MANUFACTURE_NAME 0x20 // manufacturer name + * #define BATTMONITOR_SMBUS_SOLO_DEVICE_NAME 0x21 // device name + * #define BATTMONITOR_SMBUS_SOLO_DEVICE_CHEMISTRY 0x22 // device chemistry + * #define BATTMONITOR_SMBUS_SOLO_MANUFACTURE_INFO 0x25 // manufacturer info including cell voltage + * #define BATTMONITOR_SMBUS_SOLO_CELL_VOLTAGE 0x28 // cell voltage register + */ + +// Constructor +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)) +{ + _dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_SMBus_Solo::timer, void)); +} + +/// Read the battery voltage and current. Should be called at 10hz +void AP_BattMonitor_SMBus_Solo::read() +{ + // nothing to do - all done in timer() +} + +void AP_BattMonitor_SMBus_Solo::timer() +{ + uint16_t data; + uint8_t buff[4]; + uint32_t tnow = AP_HAL::micros(); + + // read voltage + if (read_word(BATTMONITOR_SMBUS_SOLO_VOLTAGE, data)) { + _state.voltage = (float)data / 1000.0f; + _state.last_time_micros = tnow; + _state.healthy = true; + } + + // timeout after 5 seconds + if ((tnow - _state.last_time_micros) > AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS) { + _state.healthy = false; + // do not attempt to ready any more data from battery + return; + } + + // read current + if (read_block(BATTMONITOR_SMBUS_SOLO_CURRENT, buff, 4, false) == 4) { + _state.current_amps = -(float)((int32_t)((uint32_t)buff[3]<<24 | (uint32_t)buff[2]<<16 | (uint32_t)buff[1]<<8 | (uint32_t)buff[0])) / 1000.0f; + _state.last_time_micros = tnow; + } + + // read battery design capacity + if (get_capacity() == 0) { + if (read_word(BATTMONITOR_SMBUS_SOLO_FULL_CHARGE_CAPACITY, data)) { + if (data > 0) { + set_capacity(data); + } + } + } + + // read remaining capacity + if (get_capacity() > 0) { + if (read_word(BATTMONITOR_SMBUS_SOLO_REMAINING_CAPACITY, data)) { + _state.current_total_mah = MAX(0, get_capacity() - data); + } + } + + // read the button press indicator + if (read_block(BATTMONITOR_SMBUS_SOLO_MANUFACTURE_DATA, buff, 6, false) == 6) { + bool pressed = (buff[1] >> 3) & 0x01; + + if (_button_press_count >= BATTMONITOR_SMBUS_SOLO_BUTTON_DEBOUNCE) { + // battery will power off + _state.is_powering_off = true; + + } else if (pressed) { + // battery will power off if the button is held + _button_press_count++; + + } else { + // button released, reset counters + _button_press_count = 0; + _state.is_powering_off = false; + } + AP_Notify::flags.powering_off = _state.is_powering_off; + } +} + +// 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 +{ + 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(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; +} + +#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 -#define BATTMONITOR_SBUS_I2C_BUS 1 -#define BATTMONITOR_SMBUS_I2C_ADDR 0x0B // default I2C bus address - -class AP_BattMonitor_SMBus_I2C : public AP_BattMonitor_SMBus +class AP_BattMonitor_SMBus_Solo : public AP_BattMonitor_SMBus { public: // Constructor - AP_BattMonitor_SMBus_I2C(AP_BattMonitor &mon, uint8_t instance, + AP_BattMonitor_SMBus_Solo(AP_BattMonitor &mon, uint8_t instance, AP_BattMonitor::BattMonitor_State &mon_state, AP_HAL::OwnPtr dev); - // Read the battery voltage and current. Should be called at 10hz - void read(); + // read does nothing, all done in timer + void read() override; private: @@ -37,4 +34,5 @@ private: 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; + uint8_t _button_press_count; };