diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 6c402c5093..eb086a755f 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -3,6 +3,7 @@ #include "AP_BattMonitor_SMBus.h" #include "AP_BattMonitor_Bebop.h" #include "AP_BattMonitor_BLHeliESC.h" +#include "AP_BattMonitor_SMBus_SUI.h" #include "AP_BattMonitor_Sum.h" #include "AP_BattMonitor_FuelFlow.h" #include "AP_BattMonitor_FuelLevel_PWM.h" @@ -119,6 +120,18 @@ AP_BattMonitor::init() hal.i2c_mgr->get_device(_params[instance]._i2c_bus, AP_BATTMONITOR_SMBUS_I2C_ADDR, 100000, true, 20)); break; + case AP_BattMonitor_Params::BattMonitor_TYPE_SUI3: + _params[instance]._i2c_bus.set_default(AP_BATTMONITOR_SMBUS_BUS_INTERNAL), + drivers[instance] = new AP_BattMonitor_SMBus_SUI(*this, state[instance], _params[instance], + hal.i2c_mgr->get_device(_params[instance]._i2c_bus, AP_BATTMONITOR_SMBUS_I2C_ADDR, + 100000, true, 20), 3); + break; + case AP_BattMonitor_Params::BattMonitor_TYPE_SUI6: + _params[instance]._i2c_bus.set_default(AP_BATTMONITOR_SMBUS_BUS_INTERNAL), + drivers[instance] = new AP_BattMonitor_SMBus_SUI(*this, state[instance], _params[instance], + hal.i2c_mgr->get_device(_params[instance]._i2c_bus, AP_BATTMONITOR_SMBUS_I2C_ADDR, + 100000, true, 20), 6); + break; case AP_BattMonitor_Params::BattMonitor_TYPE_MAXELL: _params[instance]._i2c_bus.set_default(AP_BATTMONITOR_SMBUS_BUS_EXTERNAL); drivers[instance] = new AP_BattMonitor_SMBus_Maxell(*this, state[instance], _params[instance], diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index 721e0235cc..e1ec939fa2 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp @@ -13,7 +13,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = { // @Param: MONITOR // @DisplayName: Battery monitoring // @Description: Controls enabling monitoring of the battery's voltage and current - // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Maxell,8:UAVCAN-BatteryInfo,9:BLHeli ESC,10:SumOfFollowing,11:FuelFlow,12:FuelLevelPWM + // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Maxell,8:UAVCAN-BatteryInfo,9:BLHeli ESC,10:SumOfFollowing,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6 // @User: Standard // @RebootRequired: True AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, BattMonitor_TYPE_NONE, AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h index 20247d30a7..2b052f6d6b 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h @@ -25,6 +25,8 @@ public: BattMonitor_TYPE_Sum = 10, BattMonitor_TYPE_FuelFlow = 11, BattMonitor_TYPE_FuelLevel_PWM = 12, + BattMonitor_TYPE_SUI3 = 13, + BattMonitor_TYPE_SUI6 = 14, }; // low voltage sources (used for BATT_LOW_TYPE parameter) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp index bfa613d7d7..667815beaf 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp @@ -16,7 +16,7 @@ AP_BattMonitor_SMBus::AP_BattMonitor_SMBus(AP_BattMonitor &mon, void AP_BattMonitor_SMBus::init(void) { if (_dev) { - _dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_SMBus::timer, void)); + timer_handle = _dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_SMBus::timer, void)); } } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h index bad620138a..1ebb3d7055 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h @@ -49,7 +49,7 @@ public: // return true if cycle count can be provided and fills in cycles argument bool get_cycle_count(uint16_t &cycles) const override; - void init(void) override; + virtual void init(void) override; protected: @@ -94,6 +94,7 @@ protected: virtual void timer(void) = 0; // timer function to read from the battery + AP_HAL::Device::PeriodicHandle timer_handle; }; // include specific implementations diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_SUI.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_SUI.cpp new file mode 100644 index 0000000000..eea77013a2 --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_SUI.cpp @@ -0,0 +1,164 @@ +#include +#include +#include +#include +#include "AP_BattMonitor.h" +#include "AP_BattMonitor_SMBus_SUI.h" + +extern const AP_HAL::HAL& hal; + +#define REG_CELL_VOLTAGE 0x28 +#define REG_CURRENT 0x2a + +// maximum number of cells that we can read data for +#define SUI_MAX_CELL_READ 4 + +// Constructor +AP_BattMonitor_SMBus_SUI::AP_BattMonitor_SMBus_SUI(AP_BattMonitor &mon, + AP_BattMonitor::BattMonitor_State &mon_state, + AP_BattMonitor_Params ¶ms, + AP_HAL::OwnPtr dev, + uint8_t _cell_count) + : AP_BattMonitor_SMBus(mon, mon_state, params, std::move(dev)), + cell_count(_cell_count) +{ + _pec_supported = false; + _dev->set_retries(2); +} + +void AP_BattMonitor_SMBus_SUI::init(void) +{ + AP_BattMonitor_SMBus::init(); + if (_dev && timer_handle) { + // run twice as fast for two phases + _dev->adjust_periodic_callback(timer_handle, 50000); + } +} + +void AP_BattMonitor_SMBus_SUI::timer() +{ + uint32_t tnow = AP_HAL::micros(); + + // we read in two phases as the device can stall if you read + // current too rapidly after voltages + phase_voltages = !phase_voltages; + + if (phase_voltages) { + read_cell_voltages(); + update_health(); + return; + } + + // read current + int32_t current_ma; + if (read_block_bare(REG_CURRENT, (uint8_t *)¤t_ma, sizeof(current_ma))) { + _state.current_amps = current_ma * -0.001; + _state.last_time_micros = tnow; + } + + read_full_charge_capacity(); + + read_temp(); + read_serial_number(); + read_remaining_capacity(); + 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(AP_BATTMONITOR_SMBUS_I2C_ADDR, 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 +{ + // read bytes + if (!_dev->read_registers(reg, data, len)) { + return false; + } + + // return success + return true; +} + +void AP_BattMonitor_SMBus_SUI::read_cell_voltages() +{ + // read cell voltages + uint16_t voltbuff[SUI_MAX_CELL_READ]; + + if (!read_block(REG_CELL_VOLTAGE, (uint8_t *)voltbuff, sizeof(voltbuff))) { + return; + } + float pack_voltage_mv = 0.0f; + + for (uint8_t i = 0; i < MIN(SUI_MAX_CELL_READ, cell_count); i++) { + const uint16_t cell = voltbuff[i]; + _state.cell_voltages.cells[i] = cell; + pack_voltage_mv += (float)cell; + } + + if (cell_count >= SUI_MAX_CELL_READ) { + // 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 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); + const uint16_t cell_mv = (total_mv - pack_voltage_mv) / (cell_count - SUI_MAX_CELL_READ); + for (uint8_t i = SUI_MAX_CELL_READ; i < cell_count; i++) { + _state.cell_voltages.cells[i] = cell_mv; + } + pack_voltage_mv = total_mv; + } else { + // we can't get total pack voltage. Use average of cells we have so far + for (uint8_t i = SUI_MAX_CELL_READ; i < cell_count; i++) { + _state.cell_voltages.cells[i] = pack_voltage_mv / SUI_MAX_CELL_READ; + pack_voltage_mv += _state.cell_voltages.cells[i]; + } + } + } + + _has_cell_voltages = true; + + // accumulate the pack voltage out of the total of the cells + _state.voltage = pack_voltage_mv * 0.001; + last_volt_read_us = AP_HAL::micros(); +} + +/* + update healthy flag + */ +void AP_BattMonitor_SMBus_SUI::update_health() +{ + uint32_t now = AP_HAL::micros(); + _state.healthy = (now - last_volt_read_us < AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS) && + (now - _state.last_time_micros < AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS); +} diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_SUI.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_SUI.h new file mode 100644 index 0000000000..aa63997828 --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_SUI.h @@ -0,0 +1,35 @@ +#pragma once + +#include +#include +#include "AP_BattMonitor_SMBus.h" +#include + +// Base SUI class +class AP_BattMonitor_SMBus_SUI : public AP_BattMonitor_SMBus +{ +public: + + // Constructor + AP_BattMonitor_SMBus_SUI(AP_BattMonitor &mon, + AP_BattMonitor::BattMonitor_State &mon_state, + AP_BattMonitor_Params ¶ms, + AP_HAL::OwnPtr dev, + uint8_t cell_count + ); + + void init(void) override; + +private: + void timer(void) override; + 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; + bool read_block_bare(uint8_t reg, uint8_t* data, uint8_t len) const; + + const uint8_t cell_count; + bool phase_voltages; + uint32_t last_volt_read_us; +};