Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Thomas Watson 2024-11-23 19:52:24 -06:00 committed by Thomas Watson
parent cc4110140c
commit 190c3aa7ab
2 changed files with 167 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

213
libraries/AP_BattMonitor/AP_BattMonitor_INA3221.cpp
View File

@ -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; i<address_driver_count; i++) {
@ -103,22 +183,21 @@ void AP_BattMonitor_INA3221::init()
if (!d->dev) {
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<<i)) == 0) {
continue;
@ -219,35 +283,68 @@ void AP_BattMonitor_INA3221::AddressDriver::timer(void)
const uint8_t reg_shunt = 1 + channel_offset;
const uint8_t reg_bus = 2 + channel_offset;
uint16_t shunt_voltage;
if (!read_register(reg_shunt, shunt_voltage)) {
return;
uint16_t shunt_val;
if (!read_register(reg_shunt, shunt_val)) {
healthy = false;
shunt_val = 0;
}
uint16_t bus_voltage;
if (!read_register(reg_bus, bus_voltage)) {
return;
uint16_t bus_val;
if (!read_register(reg_bus, bus_val)) {
healthy = false;
bus_val = 0;
}
// transfer readings to front end:
// 2s complement number with 3 lowest bits not used, 1 count is 8mV
const float bus_voltage = ((int16_t)bus_val >> 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

14
libraries/AP_BattMonitor/AP_BattMonitor_INA3221.h
View File

@ -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