AP_BattMonitor: add support for ina3221 triple-channel current/voltage sensor

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -21,6 +21,7 @@
 #include "AP_BattMonitor_EFI.h"
 #include "AP_BattMonitor_INA2xx.h"
 #include "AP_BattMonitor_INA239.h"
+#include "AP_BattMonitor_INA3221.h"
 #include "AP_BattMonitor_LTC2946.h"
 #include "AP_BattMonitor_Torqeedo.h"
 #include "AP_BattMonitor_FuelLevel_Analog.h"
@@ -604,6 +605,11 @@ AP_BattMonitor::init()
                 drivers[instance] = NEW_NOTHROW AP_BattMonitor_Scripting(*this, state[instance], _params[instance]);
                 break;
 #endif // AP_BATTERY_SCRIPTING_ENABLED
+#if AP_BATTERY_INA3221_ENABLED
+            case Type::INA3221:
+                drivers[instance] = NEW_NOTHROW AP_BattMonitor_INA3221(*this, state[instance], _params[instance]);
+                break;
+#endif  // AP_BATTERY_INA3221_ENABLED
             case Type::NONE:
             default:
                 break;

libraries/AP_BattMonitor/AP_BattMonitor.h

@@ -70,6 +70,7 @@ class AP_BattMonitor
     friend class AP_BattMonitor_INA239;
     friend class AP_BattMonitor_LTC2946;
     friend class AP_BattMonitor_AD7091R5;
+    friend class AP_BattMonitor_INA3221;
 
     friend class AP_BattMonitor_Torqeedo;
     friend class AP_BattMonitor_FuelLevel_Analog;
@@ -116,6 +117,7 @@ public:
         EFI                            = 27,
         AD7091R5                       = 28,
         Scripting                      = 29,
+        INA3221                        = 30,
     };
 
     FUNCTOR_TYPEDEF(battery_failsafe_handler_fn_t, void, const char *, const int8_t);

libraries/AP_BattMonitor/AP_BattMonitor_INA3221.cpp

@@ -0,0 +1,253 @@
+#include "AP_BattMonitor_config.h"
+
+#if AP_BATTERY_INA3221_ENABLED
+
+#include "AP_BattMonitor_INA3221.h"
+
+#include <AP_HAL/utility/sparse-endian.h>
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/I2CDevice.h>
+#include <GCS_MAVLink/GCS.h>
+
+#define INA3221_DEBUGGING 0
+
+#if INA3221_DEBUGGING
+#include <stdio.h>
+#define debug(fmt, args ...)  do {printf("INA3221: %s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
+// #define debug(fmt, args ...)  do {gcs().send_text(MAV_SEVERITY_INFO, "INA3221: %s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
+#else
+#define debug(fmt, args ...)
+#endif
+
+#ifndef HAL_BATTMON_INA3221_BUS
+#define HAL_BATTMON_INA3221_BUS 0
+#endif
+
+#ifndef HAL_BATTMON_INA3221_ADDR
+#define HAL_BATTMON_INA3221_ADDR 64
+#endif
+
+struct AP_BattMonitor_INA3221::AddressDriver AP_BattMonitor_INA3221::address_driver[HAL_BATTMON_INA3221_MAX_DEVICES];
+uint8_t AP_BattMonitor_INA3221::address_driver_count;
+
+const AP_Param::GroupInfo AP_BattMonitor_INA3221::var_info[] = {
+
+    // Param indexes must be between 56 and 61 to avoid conflict with other battery monitor param tables loaded by pointer
+
+    // @Param: I2C_BUS
+    // @DisplayName: Battery monitor I2C bus number
+    // @Description: Battery monitor I2C bus number
+    // @Range: 0 3
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO("I2C_BUS", 56, AP_BattMonitor_INA3221, i2c_bus, HAL_BATTMON_INA3221_BUS),
+
+    // @Param: I2C_ADDR
+    // @DisplayName: Battery monitor I2C address
+    // @Description: Battery monitor I2C address. If this is zero then probe list of supported addresses
+    // @Range: 0 127
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO("I2C_ADDR", 57, AP_BattMonitor_INA3221, i2c_address, HAL_BATTMON_INA3221_ADDR),
+
+    // @Param: CHANNEL
+    // @DisplayName: INA3221 channel
+    // @Description: INA3221 channel to return data for
+    // @Range: 1 3
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO("CHANNEL", 58, AP_BattMonitor_INA3221, channel, 1),
+
+    AP_GROUPEND
+};
+
+extern const AP_HAL::HAL &hal;
+
+AP_BattMonitor_INA3221::AP_BattMonitor_INA3221(
+    AP_BattMonitor &mon,
+    AP_BattMonitor::BattMonitor_State &mon_state,
+    AP_BattMonitor_Params &params) :
+    AP_BattMonitor_Backend(mon, mon_state, params)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+    _state.var_info = var_info;
+}
+
+bool AP_BattMonitor_INA3221::AddressDriver::read_register(uint8_t addr, uint16_t &ret)
+{
+    if (!dev->transfer(&addr, 1, (uint8_t*)&ret, 2)) {
+        return false;
+    }
+    ret = be16toh(ret);
+    return true;
+}
+
+bool AP_BattMonitor_INA3221::AddressDriver::write_register(uint8_t addr, uint16_t val)
+{
+    uint8_t buf[3] { addr, uint8_t(val >> 8), uint8_t(val & 0xFF) };
+
+    return dev->transfer(buf, sizeof(buf), nullptr, 0);
+}
+
+#define REG_CONFIGURATION 0x00
+#define REG_MANUFACTURER_ID 0xFE
+#define REG_DIE_ID 0xFF
+void AP_BattMonitor_INA3221::init()
+{
+    debug("INA3221: probe @0x%02x on bus %u", i2c_address.get(), i2c_bus.get());
+
+    // check to see if we already have the underlying driver reading the bus:
+    for (uint8_t i=0; i<address_driver_count; i++) {
+        AddressDriver *d = &address_driver[i];
+        if (!d->dev) {
+            continue;
+        }
+        if (d->address != i2c_address.get()) {
+            continue;
+        }
+        if (d->bus != i2c_bus.get()) {
+            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) {
+            return;
+        }
+        state->channel = channel.get();
+        state->next = d->statelist;
+        state->state = &_state;
+        d->statelist = state;
+        d->channel_mask |= (1U << (uint8_t(channel.get())));
+        return;
+    }
+
+    if (address_driver_count == ARRAY_SIZE(address_driver)) {
+        debug("INA3221: out of address drivers");
+        return;
+    }
+
+    AddressDriver *d = &address_driver[address_driver_count];
+    d->dev = std::move(hal.i2c_mgr->get_device(i2c_bus, i2c_address, 100000, true, 20));
+    if (!d->dev) {
+        return;
+    }
+    d->bus = i2c_bus;
+    d->address = i2c_address;
+
+    WITH_SEMAPHORE(d->dev->get_semaphore());
+
+    // check manufacturer_id
+    uint16_t manufacturer_id;
+    if (!d->read_register(REG_MANUFACTURER_ID, manufacturer_id)) {
+        debug("read register (%u (0x%02x)) failed", REG_MANUFACTURER_ID, REG_MANUFACTURER_ID);
+        return;
+    }
+    if (manufacturer_id != 0x5449) {  // 8.6.1 p24
+        debug("Bad manufacturer_id: 0x%02x", manufacturer_id);
+        return;
+    }
+
+    uint16_t die_id;
+    if (!d->read_register(REG_DIE_ID, die_id)) {
+        debug("Bad die: 0x%02x", manufacturer_id);
+        return;
+    }
+    if (die_id != 0x3220) {  // 8.6.1 p24
+        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)) {
+        return;
+    }
+
+    AddressDriver::StateList *state = NEW_NOTHROW AddressDriver::StateList;
+    if (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())));
+
+    debug("Found INA3221 @0x%02x on bus %u", i2c_address.get(), i2c_bus.get());
+
+    address_driver_count++;
+
+    d->register_timer();
+}
+
+void AP_BattMonitor_INA3221::AddressDriver::register_timer(void)
+{
+    dev->register_periodic_callback(
+        100000,
+        FUNCTOR_BIND_MEMBER(&AP_BattMonitor_INA3221::AddressDriver::timer, void));
+}
+
+void AP_BattMonitor_INA3221::AddressDriver::timer(void)
+{
+    for (uint8_t i=1; i<=3; i++) {
+        if ((channel_mask & (1U<<i)) == 0) {
+            continue;
+        }
+        const uint8_t channel_offset = (i-1)*2;
+        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 bus_voltage;
+        if (!read_register(reg_bus, bus_voltage)) {
+            return;
+        }
+
+        // transfer readings to front end:
+        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();
+        }
+    }
+}
+
+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);
+    }
+}
+
+#endif  // AP_BATTERY_INA3221_ENABLED

libraries/AP_BattMonitor/AP_BattMonitor_INA3221.h

@@ -0,0 +1,100 @@
+#pragma once
+
+#include "AP_BattMonitor_config.h"
+
+#if AP_BATTERY_INA3221_ENABLED
+
+/*
+ *
+ * Datasheet: https://www.ti.com/lit/ds/symlink/ina3221.pdf?ts=1597369254046
+ */
+
+// The INA3221 takes two measurements for each channel: one for shunt voltage
+// and one for bus voltage. Each measurement can be independently or
+// sequentially measured, based on the mode setting (bits 2-0 in the
+// Configuration register). When the INA3221 is in normal operating mode
+// (that is, the MODE bits of the Configuration register are set to 111), the
+// device continuously converts a shunt-voltage reading followed by a
+// bus-voltage reading. This procedure converts one channel, and then continues
+// to the shunt voltage reading of the next enabled channel, followed by the
+// bus-voltage reading for that channel, and so on, until all enabled channels
+// have been measured. The programmed Configuration register mode setting
+// applies to all channels. Any channels that are not enabled are bypassed in
+// the measurement sequence, regardless of mode setting.
+
+
+// 8.3.3 Software Reset
+// The INA3221 features a software reset that reinitializes the device and
+// register settings to default power-up values without having to cycle power
+// to the device. Use bit 15 (RST) of the Configuration register to perform a
+// software reset. Setting RST reinitializes all registers and settings to the
+// default power state with the exception of the power-valid output state. If a
+// software reset is issued, the INA3221 holds the output of the PV pin until
+// the power-valid detection sequence completes. The Power-Valid UpperLimit and
+// Power-Valid Lowerlimit registers return to the default state when the
+// software reset has been issued. Therefore, any reprogrammed limit registers
+// are reset, resulting in the original power-valid thresholds validating the
+// power-valid conditions. This architecture prevents interruption to circuitry
+// connected to the powervalid output during a software reset event.
+
+// The INA3221 has programmable conversion times for both the shunt- and
+// bus-voltage measurements. The selectable conversion times for these
+// measurements range from 140μs to 8.244ms.
+
+
+#include "AP_BattMonitor.h"
+#include "AP_BattMonitor_Backend.h"
+
+#ifndef HAL_BATTMON_INA3221_MAX_DEVICES
+#define HAL_BATTMON_INA3221_MAX_DEVICES 1
+#endif
+
+class AP_BattMonitor_INA3221 : public AP_BattMonitor_Backend
+{
+public:
+    /// Constructor
+    AP_BattMonitor_INA3221(AP_BattMonitor &mon,
+                           AP_BattMonitor::BattMonitor_State &mon_state,
+                           AP_BattMonitor_Params &params);
+
+    void init() override;
+
+    /// Read the battery voltage and current.  Should be called at 10hz
+    void read() override;
+
+    bool has_current() const override {
+        return true;
+    }
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+private:
+
+    AP_Int8 i2c_bus;
+    AP_Int8 i2c_address;
+    AP_Int8 channel;
+
+    static struct AddressDriver {
+        bool read_register(uint8_t addr, uint16_t &ret);
+        bool write_register(uint8_t addr, uint16_t val);
+        void timer(void);
+        void register_timer();
+
+        AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev;
+        uint8_t bus;
+        uint8_t address;
+        uint8_t channel_mask;
+
+        struct StateList {
+            struct StateList *next;
+            uint8_t channel;
+            AP_BattMonitor::BattMonitor_State *state;
+            HAL_Semaphore sem;
+        };
+        StateList *statelist;
+
+    } address_driver[HAL_BATTMON_INA3221_MAX_DEVICES];
+    static uint8_t address_driver_count;
+};
+
+#endif  // AP_BATTERY_INA3221_ENABLED

libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp

@@ -21,7 +21,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:DroneCAN-BatteryInfo,9:ESC,10:Sum Of Selected Monitors,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:INA2XX,22:LTC2946,23:Torqeedo,24:FuelLevelAnalog,25:Synthetic Current and Analog Voltage,26:INA239_SPI,27:EFI,28:AD7091R5,29:Scripting
+    // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Generic,8:DroneCAN-BatteryInfo,9:ESC,10:Sum Of Selected Monitors,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:INA2XX,22:LTC2946,23:Torqeedo,24:FuelLevelAnalog,25:Synthetic Current and Analog Voltage,26:INA239_SPI,27:EFI,28:AD7091R5,29:Scripting,30:INA3221
     // @User: Standard
     // @RebootRequired: True
     AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, int8_t(AP_BattMonitor::Type::NONE), AP_PARAM_FLAG_ENABLE),

libraries/AP_BattMonitor/AP_BattMonitor_config.h

@@ -70,6 +70,11 @@
 #define AP_BATTERY_INA2XX_ENABLED (AP_BATTERY_BACKEND_DEFAULT_ENABLED && (BOARD_FLASH_SIZE > 1024))
 #endif
 
+#ifndef AP_BATTERY_INA3221_ENABLED
+// turned on in hwdefs (except for sim test), requires config
+#define AP_BATTERY_INA3221_ENABLED (AP_BATTERY_BACKEND_DEFAULT_ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_SITL)
+#endif
+
 #ifndef AP_BATTERY_LTC2946_ENABLED
 #define AP_BATTERY_LTC2946_ENABLED (AP_BATTERY_BACKEND_DEFAULT_ENABLED && defined(HAL_BATTMON_LTC2946_BUS) && defined(HAL_BATTMON_LTC2946_ADDR))
 #endif