From 190c3aa7abd9c24e1b66c8072e4f328e3b991408 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sat, 23 Nov 2024 19:52:24 -0600 Subject: [PATCH] AP_BattMonitor: INA3221: fix up based on real experience * correctly validate channel parameter and improve other parameter access * dynamically enable channels to avoid spending time converting unused channels * implement tracking of reading health * correct reading scaling by using datasheet values * accumulate measured current to track used mAh and Wh * make configurable using #defines (and hwdef) for integrators * correctly separate and lock frontend and backend state. Note that _state of frontend can only be accessed in `read()` method. --- .../AP_BattMonitor/AP_BattMonitor_INA3221.cpp | 213 +++++++++++++----- .../AP_BattMonitor/AP_BattMonitor_INA3221.h | 14 +- 2 files changed, 167 insertions(+), 60 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.cpp index 0deca24004..cad3a04acc 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.cpp @@ -28,6 +28,40 @@ #define HAL_BATTMON_INA3221_ADDR 64 #endif +#ifndef HAL_BATTMON_INA3221_SHUNT_OHMS +#define HAL_BATTMON_INA3221_SHUNT_OHMS 0.001 +#endif + +#define HAL_BATTMON_INA3221_CONV_TIME_140US 0b000 +#define HAL_BATTMON_INA3221_CONV_TIME_204US 0b001 +#define HAL_BATTMON_INA3221_CONV_TIME_332US 0b010 +#define HAL_BATTMON_INA3221_CONV_TIME_588US 0b011 +#define HAL_BATTMON_INA3221_CONV_TIME_1100US 0b100 +#define HAL_BATTMON_INA3221_CONV_TIME_2116US 0b101 +#define HAL_BATTMON_INA3221_CONV_TIME_4156US 0b110 +#define HAL_BATTMON_INA3221_CONV_TIME_8244US 0b111 + +#define HAL_BATTMON_INA3221_AVG_MODE_1 0b000 +#define HAL_BATTMON_INA3221_AVG_MODE_4 0b001 +#define HAL_BATTMON_INA3221_AVG_MODE_16 0b010 +#define HAL_BATTMON_INA3221_AVG_MODE_64 0b011 +#define HAL_BATTMON_INA3221_AVG_MODE_128 0b100 +#define HAL_BATTMON_INA3221_AVG_MODE_256 0b101 +#define HAL_BATTMON_INA3221_AVG_MODE_512 0b110 +#define HAL_BATTMON_INA3221_AVG_MODE_1024 0b111 + +#ifndef HAL_BATTMON_INA3221_SHUNT_CONV_TIME_SEL +#define HAL_BATTMON_INA3221_SHUNT_CONV_TIME_SEL HAL_BATTMON_INA3221_CONV_TIME_8244US +#endif + +#ifndef HAL_BATTMON_INA3221_BUS_CONV_TIME_SEL +#define HAL_BATTMON_INA3221_BUS_CONV_TIME_SEL HAL_BATTMON_INA3221_CONV_TIME_8244US +#endif + +#ifndef HAL_BATTMON_INA3221_AVG_MODE_SEL +#define HAL_BATTMON_INA3221_AVG_MODE_SEL HAL_BATTMON_INA3221_AVG_MODE_1024 +#endif + struct AP_BattMonitor_INA3221::AddressDriver AP_BattMonitor_INA3221::address_driver[HAL_BATTMON_INA3221_MAX_DEVICES]; uint8_t AP_BattMonitor_INA3221::address_driver_count; @@ -93,9 +127,55 @@ bool AP_BattMonitor_INA3221::AddressDriver::write_register(uint8_t addr, uint16_ #define REG_CONFIGURATION 0x00 #define REG_MANUFACTURER_ID 0xFE #define REG_DIE_ID 0xFF + +bool AP_BattMonitor_INA3221::AddressDriver::write_config(void) +{ + // update device configuration + union { + struct PACKED { + uint16_t mode : 3; + uint16_t shunt_voltage_conversiontime : 3; + uint16_t bus_voltage_conversiontime : 3; + uint16_t averaging_mode : 3; + uint16_t ch1_enable : 1; + uint16_t ch2_enable : 1; + uint16_t ch3_enable : 1; + uint16_t reset : 1; + } bits; + uint16_t word; + } configuration {{ + 0b111, // continuous operation + HAL_BATTMON_INA3221_SHUNT_CONV_TIME_SEL, + HAL_BATTMON_INA3221_BUS_CONV_TIME_SEL, + HAL_BATTMON_INA3221_AVG_MODE_SEL, + // dynamically enable channels to not waste time converting unused data + (channel_mask & (1 << 1)) != 0, // enable ch1? + (channel_mask & (1 << 2)) != 0, // enable ch2? + (channel_mask & (1 << 3)) != 0, // enable ch3? + 0b0, // don't reset... + }}; + + if (!write_register(REG_CONFIGURATION, configuration.word)) { + return false; + } + + dev_channel_mask = channel_mask; // what's actually in the device now + + return true; +} + void AP_BattMonitor_INA3221::init() { - debug("INA3221: probe @0x%02x on bus %u", i2c_address.get(), i2c_bus.get()); + uint8_t dev_address = i2c_address.get(); + uint8_t dev_bus = i2c_bus.get(); + uint8_t dev_channel = channel.get(); + + if ((dev_channel < 1) || (dev_channel > 3)) { + debug("INA3221: nonexistent channel"); + return; + } + + debug("INA3221: probe ch%d@0x%02x on bus %u", dev_channel, dev_address, dev_bus); // check to see if we already have the underlying driver reading the bus: for (uint8_t i=0; idev) { continue; } - if (d->address != i2c_address.get()) { + if (d->address != dev_address) { continue; } - if (d->bus != i2c_bus.get()) { + if (d->bus != dev_bus) { continue; } - debug("Reusing INA3221 driver @0x%02x on bus %u", i2c_address.get(), i2c_bus.get()); - AddressDriver::StateList *state = NEW_NOTHROW AddressDriver::StateList; - if (state == nullptr) { + debug("Reusing INA3221 driver @0x%02x on bus %u", dev_address, dev_bus); + address_driver_state = NEW_NOTHROW AddressDriver::StateList; + if (address_driver_state == nullptr) { return; } - state->channel = channel.get(); - state->next = d->statelist; - state->state = &_state; - d->statelist = state; - d->channel_mask |= (1U << (uint8_t(channel.get()))); + address_driver_state->channel = dev_channel; + address_driver_state->next = d->statelist; + d->statelist = address_driver_state; + d->channel_mask |= (1 << dev_channel); return; } @@ -157,45 +236,20 @@ void AP_BattMonitor_INA3221::init() return; } - // reset the device: - union { - struct PACKED { - uint16_t mode : 3; - uint16_t shunt_voltage_conversiontime : 3; - uint16_t bus_voltage_conversiontime : 3; - uint16_t averaging_mode : 3; - uint16_t ch1_enable : 1; - uint16_t ch2_enable : 1; - uint16_t ch3_enable : 1; - uint16_t reset : 1; - } bits; - uint16_t word; - } configuration { - 0b111, // continuous operation - 0b111, // 8ms conversion time for shunt voltage - 0b111, // 8ms conversion time for bus voltage - 0b111, // 1024 samples / average - 0b1, // enable ch1 - 0b1, // enable ch2 - 0b1, // enable ch3 - 0b0 // don't reset... - }; - - if (!d->write_register(REG_CONFIGURATION, configuration.word)) { + d->channel_mask = (1 << dev_channel); + if (!d->write_config()) { return; } - AddressDriver::StateList *state = NEW_NOTHROW AddressDriver::StateList; - if (state == nullptr) { + address_driver_state = NEW_NOTHROW AddressDriver::StateList; + if (address_driver_state == nullptr) { return; } - state->channel = channel.get(); - state->next = d->statelist; - state->state = &_state; - d->statelist = state; - d->channel_mask |= (1U << (uint8_t(channel.get()))); + address_driver_state->channel = dev_channel; + address_driver_state->next = d->statelist; + d->statelist = address_driver_state; - debug("Found INA3221 @0x%02x on bus %u", i2c_address.get(), i2c_bus.get()); + debug("Found INA3221 ch%d@0x%02x on bus %u", dev_channel, dev_address, dev_bus); address_driver_count++; @@ -211,6 +265,16 @@ void AP_BattMonitor_INA3221::AddressDriver::register_timer(void) void AP_BattMonitor_INA3221::AddressDriver::timer(void) { + bool healthy = true; + + if (channel_mask != dev_channel_mask) { + if (write_config()) { // update enabled channels + return; // data is now out of date, read it next time + } + // continue on to reading if update failed so health gets cleared + healthy = false; + } + for (uint8_t i=1; i<=3; i++) { if ((channel_mask & (1U<> 3)*8e-3; + // 2s complement number with 3 lowest bits not used, 1 count is 40uV + const float shunt_voltage = ((int16_t)shunt_val >> 3)*40e-6; + const float shunt_resistance = HAL_BATTMON_INA3221_SHUNT_OHMS; + const float shunt_current = shunt_voltage/shunt_resistance; // I = V/R + + // transfer readings to state for (auto *state = statelist; state != nullptr; state = state->next) { if (state->channel != i) { continue; } WITH_SEMAPHORE(state->sem); - state->state->voltage = bus_voltage/32768.0 * 26; - state->state->current_amps = shunt_voltage * 0.56f; - state->state->last_time_micros = AP_HAL::micros(); + + // calculate time since last data read + const uint32_t tnow = AP_HAL::micros(); + const uint32_t dt_us = tnow - state->last_time_micros; + + state->healthy = healthy; + state->voltage = bus_voltage; + state->current_amps = shunt_current; + + // update current drawn since last reading for read to accumulate + if (state->last_time_micros != 0 && dt_us < 2000000) { + const float mah = calculate_mah(state->current_amps, dt_us); + state->delta_mah += mah; + state->delta_wh += 0.001 * mah * state->voltage; + } + + state->last_time_micros = tnow; } } } void AP_BattMonitor_INA3221::read() { - static uint32_t last_print; - if (AP_HAL::millis() - last_print > 5000) { - last_print = AP_HAL::millis(); - gcs().send_text(MAV_SEVERITY_INFO, "%u: voltage:%f current:%f", (unsigned)_state.last_time_micros, _state.voltage, _state.current_amps); + if (address_driver_state == nullptr) { + return; } + + WITH_SEMAPHORE(address_driver_state->sem); + // copy state data to front-end under semaphore + _state.healthy = address_driver_state->healthy; + _state.voltage = address_driver_state->voltage; + _state.current_amps = address_driver_state->current_amps; + _state.consumed_mah += address_driver_state->delta_mah; + _state.consumed_wh += address_driver_state->delta_wh; + _state.last_time_micros = address_driver_state->last_time_micros; + + address_driver_state->delta_mah = 0; + address_driver_state->delta_wh = 0; } #endif // AP_BATTERY_INA3221_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.h b/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.h index d551800960..898753207c 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.h @@ -77,6 +77,7 @@ private: static struct AddressDriver { bool read_register(uint8_t addr, uint16_t &ret); bool write_register(uint8_t addr, uint16_t val); + bool write_config(void); void timer(void); void register_timer(); @@ -84,17 +85,26 @@ private: uint8_t bus; uint8_t address; uint8_t channel_mask; + uint8_t dev_channel_mask; struct StateList { struct StateList *next; - uint8_t channel; - AP_BattMonitor::BattMonitor_State *state; HAL_Semaphore sem; + uint8_t channel; + + bool healthy; + float voltage; + float current_amps; + float delta_mah; + float delta_wh; + uint32_t last_time_micros; }; StateList *statelist; } address_driver[HAL_BATTMON_INA3221_MAX_DEVICES]; static uint8_t address_driver_count; + + AddressDriver::StateList *address_driver_state; }; #endif // AP_BATTERY_INA3221_ENABLED