AP_BattMonitor: support INA228 and INA238 battery monitor

I2C monitors with similar functionality to INA226

libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp

@@ -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
+// INA226 specific registers
-#define REG_CONFIG_RESET   0x8000
+#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());
+    switch (dtype) {
+    case DevType::UNKNOWN:
+        return false;
 
-    int16_t config = 0;
+    case DevType::INA226: {
-    if (!write_word(REG_CONFIG, REG_CONFIG_RESET) ||
+        // configure for MAX_AMPS
-        !write_word(REG_CONFIG, REG_CONFIG_DEFAULT) ||
+        const uint16_t conf = (0x2<<9) | (0x5<<6) | (0x5<<3) | 0x7; // 2ms conv time, 16x sampling
-        !read_word(REG_CONFIG, config) ||
+        const float rShunt = 0.0005;
-        config != REG_CONFIG_DEFAULT) {
+        current_LSB = MAX_AMPS / 32768.0;
-        return;
+        voltage_LSB = 0.00125; // 1.25mV/bit
+        const uint16_t cal = uint16_t(0.00512 / (current_LSB * rShunt));
+        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;
     }
 
-    // configure for MAX_AMPS
+    case DevType::INA228: {
-    const uint16_t conf = (0x2<<9) | (0x5<<6) | (0x5<<3) | 0x7; // 2ms conv time, 16x sampling
+        // configure for MAX_AMPS
-    const float rShunt = 0.0005;
+        voltage_LSB = 195.3125e-6; // 195.3125 uV/LSB
-    current_LSB = MAX_AMPS / 32768.0;
+        const float rShunt = 0.0005;
-    voltage_LSB = 0.00125; // 1.25mV/bit
+        current_LSB = MAX_AMPS / (1<<19);
-    const uint16_t cal = uint16_t(0.00512 / (current_LSB * rShunt));
+        const uint16_t shunt_cal = uint16_t(13107.2e6 * current_LSB * rShunt) & 0x7FFF;
-    if (!write_word(REG_CONFIG, REG_CONFIG_RESET) || // reset
+        if (write_word(REG_228_CONFIG, REG_228_CONFIG_RESET) && // reset
-        !write_word(REG_CONFIG, conf) ||
+            write_word(REG_228_CONFIG, 0) &&
-        !write_word(REG_CALIBRATION, cal)) {
+            write_word(REG_228_SHUNT_CAL, shunt_cal)) {
-        return;
+            dev_type = dtype;
+            return true;
+        }
+        break;
     }
 
-    configured = true;
+    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
+  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 (dev_type == DevType::UNKNOWN) {
-    if (!configured) {
+        if (!detect_device()) {
-        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
             return;
         }
     }
 
-    int16_t bus_voltage, current;
+    float voltage = 0, current = 0;
 
-    if (!read_word(REG_BUS_VOLTAGE, bus_voltage) ||
+    switch (dev_type) {
-        !read_word(REG_CURRENT, current)) {
+    case DevType::UNKNOWN:
-        failed_reads++;
-        if (failed_reads > 10) {
-            // device has disconnected, we need to reconfigure it
-            configured = false;
-        }
         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
+                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.volt_sum += voltage;
-    accumulate.current_sum += current * current_LSB;
+    accumulate.current_sum += current;
     accumulate.count++;
 }
 

libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.h

@@ -30,17 +30,27 @@ public:
 private:
     AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev;
 
-    void configure(void);
+    enum class DevType : uint8_t {
-    bool read_word(const uint8_t reg, int16_t& data) const;
+        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;