From 281b8eb234fb50e783950528c5e41dd464b97d69 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 10 Oct 2021 19:56:41 +1100 Subject: [PATCH] AP_BattMonitor: support LTC2946 I2C battery monitoring --- libraries/AP_BattMonitor/AP_BattMonitor.cpp | 6 + libraries/AP_BattMonitor/AP_BattMonitor.h | 3 + .../AP_BattMonitor/AP_BattMonitor_LTC2946.cpp | 114 ++++++++++++++++++ .../AP_BattMonitor/AP_BattMonitor_LTC2946.h | 43 +++++++ .../AP_BattMonitor/AP_BattMonitor_Params.cpp | 2 +- 5 files changed, 167 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.cpp create mode 100644 libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.h diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 44d7d1c19e..eeaa6018a5 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -15,6 +15,7 @@ #include "AP_BattMonitor_Generator.h" #include "AP_BattMonitor_MPPT_PacketDigital.h" #include "AP_BattMonitor_INA231.h" +#include "AP_BattMonitor_LTC2946.h" #include @@ -300,6 +301,11 @@ AP_BattMonitor::init() case Type::INA231: drivers[instance] = new AP_BattMonitor_INA231(*this, state[instance], _params[instance]); break; +#endif +#if HAL_BATTMON_LTC2946_ENABLED + case Type::LTC2946: + drivers[instance] = new AP_BattMonitor_LTC2946(*this, state[instance], _params[instance]); + break; #endif case Type::NONE: default: diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index 8f00fdfa81..7828b685e9 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -47,6 +47,7 @@ class AP_BattMonitor_UAVCAN; class AP_BattMonitor_Generator; class AP_BattMonitor_MPPT_PacketDigital; class AP_BattMonitor_INA231; +class AP_BattMonitor_LTC2946; class AP_BattMonitor { @@ -64,6 +65,7 @@ class AP_BattMonitor friend class AP_BattMonitor_Generator; friend class AP_BattMonitor_MPPT_PacketDigital; friend class AP_BattMonitor_INA231; + friend class AP_BattMonitor_LTC2946; public: @@ -96,6 +98,7 @@ public: Rotoye = 19, MPPT_PacketDigital = 20, INA231 = 21, + LTC2946 = 22, }; FUNCTOR_TYPEDEF(battery_failsafe_handler_fn_t, void, const char *, const int8_t); diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.cpp new file mode 100644 index 0000000000..418698436f --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.cpp @@ -0,0 +1,114 @@ +#include "AP_BattMonitor_LTC2946.h" +#include +#include + +#if HAL_BATTMON_LTC2946_ENABLED + +extern const AP_HAL::HAL& hal; + +#define REG_CTRLA 0x00 +#define REG_CTRLB 0x01 +#define REG_STATUS 0x80 +#define REG_MFR_ID 0xe7 + +// first byte of 16 bit ID is stable +#define ID_LTC2946 0x60 + +#define REG_DELTA 0x14 // 16 bits +#define REG_VIN 0x1e // 16 bits + +#define REGA_CONF 0x18 // sense, alternate +#define REGB_CONF 0x01 // auto-reset + +void AP_BattMonitor_LTC2946::init(void) +{ + dev = hal.i2c_mgr->get_device(HAL_BATTMON_LTC2946_BUS, HAL_BATTMON_LTC2946_ADDR, 100000, false, 20); + if (!dev) { + return; + } + + uint8_t id = 0; + WITH_SEMAPHORE(dev->get_semaphore()); + if (!dev->read_registers(REG_MFR_ID, &id, 1) || id != ID_LTC2946) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "LTC2946: Failed to find device 0x%04x", unsigned(id)); + return; + } + + if (!dev->write_register(REG_CTRLA, REGA_CONF) || + !dev->write_register(REG_CTRLB, REGB_CONF)) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "LTC2946: Failed to configure device"); + return; + } + + // use datasheet typical values + voltage_LSB = 102.4 / 4095.0; + current_LSB = (0.1024/0.0005) / 4095.0; + + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "LTC2946: found monitor on bus %u", HAL_BATTMON_LTC2946_BUS); + + if (dev) { + dev->register_periodic_callback(25000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_LTC2946::timer, void)); + } +} + +/// read the battery_voltage and current, should be called at 10hz +void AP_BattMonitor_LTC2946::read(void) +{ + WITH_SEMAPHORE(accumulate.sem); + _state.healthy = accumulate.count > 0; + if (!_state.healthy) { + return; + } + + _state.voltage = accumulate.volt_sum / accumulate.count; + _state.current_amps = accumulate.current_sum / accumulate.count; + accumulate.volt_sum = 0; + accumulate.current_sum = 0; + accumulate.count = 0; + + const uint32_t tnow = AP_HAL::micros(); + const float dt = tnow - _state.last_time_micros; + + // update total current drawn since startup + if (_state.last_time_micros != 0 && dt < 2000000.0) { + // .0002778 is 1/3600 (conversion to hours) + const float mah = _state.current_amps * dt * 0.0000002778; + _state.consumed_mah += mah; + _state.consumed_wh += 0.001 * mah * _state.voltage; + } + _state.last_time_micros = tnow; +} + +/* + read word from register + returns true if read was successful, false if failed +*/ +bool AP_BattMonitor_LTC2946::read_word(const uint8_t reg, uint16_t& data) const +{ + // read the appropriate register from the device + if (!dev->read_registers(reg, (uint8_t *)&data, sizeof(data))) { + return false; + } + + // convert byte order + data = uint16_t(be16toh(uint16_t(data))); + + // return success + return true; +} + +void AP_BattMonitor_LTC2946::timer(void) +{ + uint16_t amps, volts; + if (!read_word(REG_DELTA, amps) || + !read_word(REG_VIN, volts)) { + return; + } + WITH_SEMAPHORE(accumulate.sem); + // convert 12 bit ADC data + accumulate.volt_sum += (volts>>4) * voltage_LSB; + accumulate.current_sum += (amps>>4) * current_LSB; + accumulate.count++; +} + +#endif // HAL_BATTMON_LTC2946_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.h b/libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.h new file mode 100644 index 0000000000..c3497e9df8 --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.h @@ -0,0 +1,43 @@ +#pragma once + +#include +#include +#include "AP_BattMonitor_Backend.h" +#include + +#define HAL_BATTMON_LTC2946_ENABLED defined(HAL_BATTMON_LTC2946_BUS) && defined(HAL_BATTMON_LTC2946_ADDR) + +#if HAL_BATTMON_LTC2946_ENABLED + +class AP_BattMonitor_LTC2946 : public AP_BattMonitor_Backend +{ +public: + // inherit constructor + using AP_BattMonitor_Backend::AP_BattMonitor_Backend; + + bool has_cell_voltages() const override { return false; } + bool has_temperature() const override { return false; } + bool has_current() const override { return true; } + bool reset_remaining(float percentage) override { return false; } + bool get_cycle_count(uint16_t &cycles) const override { return false; } + + virtual void init(void) override; + virtual void read() override; + +private: + AP_HAL::OwnPtr dev; + + bool read_word(const uint8_t reg, uint16_t& data) const; + void timer(void); + + struct { + uint16_t count; + float volt_sum; + float current_sum; + HAL_Semaphore sem; + } accumulate; + float current_LSB; + float voltage_LSB; +}; + +#endif // HAL_BATTMON_LTC2946_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index d16798f212..25325bc906 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-Generic,8:UAVCAN-BatteryInfo,9:ESC,10:SumOfFollowing,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell,17:Generator-Elec,18:Generator-Fuel,19:Rotoye,20:MPPT,21:INA231 + // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Generic,8:UAVCAN-BatteryInfo,9:ESC,10:SumOfFollowing,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell,17:Generator-Elec,18:Generator-Fuel,19:Rotoye,20:MPPT,21:INA231,22:LTC2946 // @User: Standard // @RebootRequired: True AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, int8_t(AP_BattMonitor::Type::NONE), AP_PARAM_FLAG_ENABLE),