Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 00:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_BattMonitor: support INA228 and INA238 battery monitor

Browse Source 
I2C monitors with similar functionality to INA226
This commit is contained in:
Andrew Tridgell 2023-04-29 09:58:55 +10:00
parent 2b45b235ad
commit 5227b99a39
2 changed files with 207 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

224
libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp
View File

@ -2,20 +2,45 @@
#if AP_BATTERY_INA2XX_ENABLED
/*
supports INA226, INA228 and INA238 I2C battery monitors
*/
#include <AP_HAL/utility/sparse-endian.h>
#include "AP_BattMonitor_INA2xx.h"
extern const AP_HAL::HAL& hal;
#define REG_CONFIG 0x00
#define REG_SHUNT_VOLTAGE 0x01
#define REG_BUS_VOLTAGE 0x02
#define REG_CURRENT 0x04
#define REG_CALIBRATION 0x05
#define REG_CONFIG_DEFAULT 0x4127
#define REG_CONFIG_RESET 0x8000
// INA226 specific registers
#define REG_226_CONFIG 0x00
#define REG_226_CONFIG_DEFAULT 0x4127
#define REG_226_CONFIG_RESET 0x8000
#define REG_226_BUS_VOLTAGE 0x02
#define REG_226_CURRENT 0x04
#define REG_226_CALIBRATION 0x05
#define REG_226_MANUFACT_ID 0xfe
// INA228 specific registers
#define REG_228_CONFIG 0x00
#define REG_228_CONFIG_RESET 0x8000
#define REG_228_ADC_CONFIG 0x01
#define REG_228_SHUNT_CAL 0x02
#define REG_228_VBUS 0x05
#define REG_228_CURRENT 0x07
#define REG_228_MANUFACT_ID 0x3e
#define REG_228_DEVICE_ID 0x3f
// INA238 specific registers
#define REG_238_CONFIG 0x00
#define REG_238_CONFIG_RESET 0x8000
#define REG_238_ADC_CONFIG 0x01
#define REG_238_SHUNT_CAL 0x02
#define REG_238_VBUS 0x05
#define REG_238_CURRENT 0x07
#define REG_238_MANUFACT_ID 0x3e
#define REG_238_DEVICE_ID 0x3f
// this should become a parameter in future
#define MAX_AMPS 90.0
@ -67,31 +92,60 @@ void AP_BattMonitor_INA2XX::init(void)
dev->register_periodic_callback(25000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_INA2XX::timer, void));
}
void AP_BattMonitor_INA2XX::configure(void)
bool AP_BattMonitor_INA2XX::configure(DevType dtype)
{
WITH_SEMAPHORE(dev->get_semaphore());
int16_t config = 0;
if (!write_word(REG_CONFIG, REG_CONFIG_RESET) ||
!write_word(REG_CONFIG, REG_CONFIG_DEFAULT) ||
!read_word(REG_CONFIG, config) ||
config != REG_CONFIG_DEFAULT) {
return;
}
switch (dtype) {
case DevType::UNKNOWN:
return false;
case DevType::INA226: {
// configure for MAX_AMPS
const uint16_t conf = (0x2<<9) | (0x5<<6) | (0x5<<3) | 0x7; // 2ms conv time, 16x sampling
const float rShunt = 0.0005;
current_LSB = MAX_AMPS / 32768.0;
voltage_LSB = 0.00125; // 1.25mV/bit
const uint16_t cal = uint16_t(0.00512 / (current_LSB * rShunt));
if (!write_word(REG_CONFIG, REG_CONFIG_RESET) || // reset
!write_word(REG_CONFIG, conf) ||
!write_word(REG_CALIBRATION, cal)) {
return;
if (write_word(REG_226_CONFIG, REG_226_CONFIG_RESET) && // reset
write_word(REG_226_CONFIG, conf) &&
write_word(REG_226_CALIBRATION, cal)) {
dev_type = dtype;
return true;
}
break;
}
configured = true;
case DevType::INA228: {
// configure for MAX_AMPS
voltage_LSB = 195.3125e-6; // 195.3125 uV/LSB
const float rShunt = 0.0005;
current_LSB = MAX_AMPS / (1<<19);
const uint16_t shunt_cal = uint16_t(13107.2e6 * current_LSB * rShunt) & 0x7FFF;
if (write_word(REG_228_CONFIG, REG_228_CONFIG_RESET) && // reset
write_word(REG_228_CONFIG, 0) &&
write_word(REG_228_SHUNT_CAL, shunt_cal)) {
dev_type = dtype;
return true;
}
break;
}
case DevType::INA238: {
// configure for MAX_AMPS
voltage_LSB = 3.125e-3; // 3.125mV/LSB
const float rShunt = 0.0005;
current_LSB = MAX_AMPS / (1<<15);
const uint16_t shunt_cal = uint16_t(819.2e6 * current_LSB * rShunt) & 0x7FFF;
if (write_word(REG_238_CONFIG, REG_238_CONFIG_RESET) && // reset
write_word(REG_238_CONFIG, 0) &&
write_word(REG_238_SHUNT_CAL, shunt_cal)) {
dev_type = dtype;
return true;
}
break;
}
}
return false;
}
/// read the battery_voltage and current, should be called at 10hz
@ -119,10 +173,10 @@ void AP_BattMonitor_INA2XX::read(void)
}
/*
read word from register
read 16 bit word from register
returns true if read was successful, false if failed
*/
bool AP_BattMonitor_INA2XX::read_word(const uint8_t reg, int16_t& data) const
bool AP_BattMonitor_INA2XX::read_word16(const uint8_t reg, int16_t& data) const
{
// read the appropriate register from the device
if (!dev->read_registers(reg, (uint8_t *)&data, sizeof(data))) {
@ -135,6 +189,25 @@ bool AP_BattMonitor_INA2XX::read_word(const uint8_t reg, int16_t& data) const
return true;
}
/*
read 24 bit signed value from register
returns true if read was successful, false if failed
*/
bool AP_BattMonitor_INA2XX::read_word24(const uint8_t reg, int32_t& data) const
{
// read the appropriate register from the device
uint8_t d[3];
if (!dev->read_registers(reg, d, sizeof(d))) {
return false;
}
// 24 bit 2s complement data. Shift into upper 24 bits of int32_t then divide by 256
// to cope with negative numbers properly
data = d[0]<<24 | d[1]<<16 | d[2] << 8;
data = data / 256;
return true;
}
/*
write word to a register, byte swapped
returns true if write was successful, false if failed
@ -145,38 +218,109 @@ bool AP_BattMonitor_INA2XX::write_word(const uint8_t reg, const uint16_t data) c
return dev->transfer(b, sizeof(b), nullptr, 0);
}
/*
detect device type. This may happen well after power on if battery is
not plugged in yet
*/
bool AP_BattMonitor_INA2XX::detect_device(void)
{
uint32_t now = AP_HAL::millis();
if (now - last_detect_ms < 200) {
// don't flood the bus
return false;
}
last_detect_ms = now;
int16_t id;
WITH_SEMAPHORE(dev->get_semaphore());
if (read_word16(REG_228_MANUFACT_ID, id) && id == 0x5449 &&
read_word16(REG_228_DEVICE_ID, id) && (id&0xFFF0) == 0x2280) {
return configure(DevType::INA228);
}
if (read_word16(REG_238_MANUFACT_ID, id) && id == 0x5449 &&
read_word16(REG_238_DEVICE_ID, id) && (id&0xFFF0) == 0x2380) {
return configure(DevType::INA238);
}
if (read_word16(REG_226_MANUFACT_ID, id) && id == 0x5449 &&
write_word(REG_226_CONFIG, REG_226_CONFIG_RESET) &&
write_word(REG_226_CONFIG, REG_226_CONFIG_DEFAULT) &&
read_word16(REG_226_CONFIG, id) &&
id == REG_226_CONFIG_DEFAULT) {
return configure(DevType::INA226);
}
return false;
}
void AP_BattMonitor_INA2XX::timer(void)
{
// allow for power-on after boot
if (!configured) {
uint32_t now = AP_HAL::millis();
if (now - last_configure_ms > 200) {
// try contacting the device at 5Hz
last_configure_ms = now;
configure();
}
if (!configured) {
// waiting for the device to respond
if (dev_type == DevType::UNKNOWN) {
if (!detect_device()) {
return;
}
}
int16_t bus_voltage, current;
float voltage = 0, current = 0;
if (!read_word(REG_BUS_VOLTAGE, bus_voltage) ||
!read_word(REG_CURRENT, current)) {
switch (dev_type) {
case DevType::UNKNOWN:
return;
case DevType::INA226: {
int16_t bus_voltage16, current16;
if (!read_word16(REG_226_BUS_VOLTAGE, bus_voltage16) ||
!read_word16(REG_226_CURRENT, current16)) {
failed_reads++;
if (failed_reads > 10) {
// device has disconnected, we need to reconfigure it
configured = false;
dev_type = DevType::UNKNOWN;
}
return;
}
voltage = bus_voltage16 * voltage_LSB;
current = current16 * current_LSB;
break;
}
case DevType::INA228: {
int32_t bus_voltage24, current24;
if (!read_word24(REG_228_VBUS, bus_voltage24) ||
!read_word24(REG_228_CURRENT, current24)) {
failed_reads++;
if (failed_reads > 10) {
// device has disconnected, we need to reconfigure it
dev_type = DevType::UNKNOWN;
}
return;
}
voltage = (bus_voltage24>>4) * voltage_LSB;
current = (current24>>4) * current_LSB;
break;
}
case DevType::INA238: {
int16_t bus_voltage16, current16;
if (!read_word16(REG_238_VBUS, bus_voltage16) ||
!read_word16(REG_238_CURRENT, current16)) {
failed_reads++;
if (failed_reads > 10) {
// device has disconnected, we need to reconfigure it
dev_type = DevType::UNKNOWN;
}
return;
}
voltage = bus_voltage16 * voltage_LSB;
current = current16 * current_LSB;
break;
}
}
failed_reads = 0;
WITH_SEMAPHORE(accumulate.sem);
accumulate.volt_sum += bus_voltage * voltage_LSB;
accumulate.current_sum += current * current_LSB;
accumulate.volt_sum += voltage;
accumulate.current_sum += current;
accumulate.count++;
}

18
libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.h
View File

@ -30,17 +30,27 @@ public:
private:
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev;
void configure(void);
bool read_word(const uint8_t reg, int16_t& data) const;
enum class DevType : uint8_t {
UNKNOWN = 0,
INA226,
INA228,
INA238,
};
bool configure(DevType dtype);
bool read_word16(const uint8_t reg, int16_t& data) const;
bool read_word24(const uint8_t reg, int32_t& data) const;
bool write_word(const uint8_t reg, const uint16_t data) const;
void timer(void);
bool detect_device(void);
DevType dev_type;
uint32_t last_detect_ms;
AP_Int8 i2c_bus;
AP_Int8 i2c_address;
bool configured;
bool callback_registered;
uint32_t failed_reads;
uint32_t last_configure_ms;
struct {
uint16_t count;