AP_BattMonitor: added support for INA239 SPI battery monitor

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -14,6 +14,7 @@
 #include "AP_BattMonitor_FuelLevel_PWM.h"
 #include "AP_BattMonitor_Generator.h"
 #include "AP_BattMonitor_INA2xx.h"
+#include "AP_BattMonitor_INA239.h"
 #include "AP_BattMonitor_LTC2946.h"
 #include "AP_BattMonitor_Torqeedo.h"
 #include "AP_BattMonitor_FuelLevel_Analog.h"
@@ -347,6 +348,11 @@ AP_BattMonitor::init()
             case Type::Analog_Volt_Synthetic_Current:
                 drivers[instance] = new AP_BattMonitor_Synthetic_Current(*this, state[instance], _params[instance]);
                 break;
+#endif
+#if HAL_BATTMON_INA239_ENABLED
+            case Type::INA239_SPI:
+                drivers[instance] = new AP_BattMonitor_INA239(*this, state[instance], _params[instance]);
+                break;
 #endif
             case Type::NONE:
             default:

libraries/AP_BattMonitor/AP_BattMonitor.h

@@ -59,6 +59,7 @@ class AP_BattMonitor_SMBus_Rotoye;
 class AP_BattMonitor_UAVCAN;
 class AP_BattMonitor_Generator;
 class AP_BattMonitor_INA2XX;
+class AP_BattMonitor_INA239;
 class AP_BattMonitor_LTC2946;
 class AP_BattMonitor_Torqeedo;
 class AP_BattMonitor_FuelLevel_Analog;
@@ -79,6 +80,7 @@ class AP_BattMonitor
     friend class AP_BattMonitor_FuelLevel_PWM;
     friend class AP_BattMonitor_Generator;
     friend class AP_BattMonitor_INA2XX;
+    friend class AP_BattMonitor_INA239;
     friend class AP_BattMonitor_LTC2946;
 
     friend class AP_BattMonitor_Torqeedo;
@@ -120,6 +122,7 @@ public:
         Torqeedo                       = 23,
         FuelLevel_Analog               = 24,
         Analog_Volt_Synthetic_Current  = 25,
+        INA239_SPI                     = 26,
     };
 
     FUNCTOR_TYPEDEF(battery_failsafe_handler_fn_t, void, const char *, const int8_t);

libraries/AP_BattMonitor/AP_BattMonitor_INA239.cpp

@@ -0,0 +1,160 @@
+#include "AP_BattMonitor_INA239.h"
+#include <GCS_MAVLink/GCS.h>
+#include <AP_HAL/utility/sparse-endian.h>
+
+#if HAL_BATTMON_INA239_ENABLED
+
+extern const AP_HAL::HAL& hal;
+
+/*
+  note that registers are clocked on SPI MSB first, with register
+  number in top 6 bits, LSB is read flag
+ */
+#define REG_CONFIG        0x00
+#define REG_ADC_CONFIG    0x01
+#define REG_SHUNT_CAL     0x02
+#define REG_SHUNT_VOLTAGE 0x04
+#define REG_BUS_VOLTAGE   0x05
+#define REG_CURRENT       0x07
+
+#define REG_ADC_CONFIG_RESET 0xFB68U
+
+#ifndef HAL_BATTMON_INA239_SHUNT_RESISTANCE
+#define HAL_BATTMON_INA239_SHUNT_RESISTANCE 0.0002
+#endif
+
+#ifndef HAL_BATTMON_INA239_MAX_CURRENT
+#define HAL_BATTMON_INA239_MAX_CURRENT 90
+#endif
+
+AP_BattMonitor_INA239::AP_BattMonitor_INA239(AP_BattMonitor &mon,
+                                             AP_BattMonitor::BattMonitor_State &mon_state,
+                                             AP_BattMonitor_Params &params)
+        : AP_BattMonitor_Backend(mon, mon_state, params)
+{
+}
+
+void AP_BattMonitor_INA239::init(void)
+{
+    dev = hal.spi->get_device(HAL_BATTMON_INA239_SPI_DEVICE);
+    if (!dev) {
+        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "device fail");
+        return;
+    }
+    dev->set_read_flag(0x01);
+    // register now and configure in the timer callbacks
+    dev->register_periodic_callback(25000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_INA239::timer, void));
+}
+
+void AP_BattMonitor_INA239::configure(void)
+{
+    WITH_SEMAPHORE(dev->get_semaphore());
+
+    int16_t adc_config = 0;
+
+    if (!read_word(REG_ADC_CONFIG, adc_config) ||
+        uint16_t(adc_config) != REG_ADC_CONFIG_RESET) {
+        return;
+    }
+
+    voltage_LSB = 3.125e-3;
+    current_LSB = float(HAL_BATTMON_INA239_MAX_CURRENT) / 0x8000;
+    const int16_t shunt_cal = 819.2 * 1e6 * current_LSB * HAL_BATTMON_INA239_SHUNT_RESISTANCE;
+    int16_t shunt_cal2 = 0;
+
+    if (!write_word(REG_SHUNT_CAL, shunt_cal) ||
+        !read_word(REG_SHUNT_CAL, shunt_cal2) ||
+        shunt_cal != shunt_cal2) {
+        return;
+    }
+
+    configured = true;
+}
+
+/// read the battery_voltage and current, should be called at 10hz
+void AP_BattMonitor_INA239::read(void)
+{
+    WITH_SEMAPHORE(accumulate.sem);
+    _state.healthy = accumulate.count > 0;
+    if (!_state.healthy) {
+        return;
+    }
+
+    _state.voltage = accumulate.volt_sum / accumulate.count;
+    _state.current_amps = accumulate.current_sum / accumulate.count;
+    accumulate.volt_sum = 0;
+    accumulate.current_sum = 0;
+    accumulate.count = 0;
+
+    const uint32_t tnow = AP_HAL::micros();
+    const uint32_t dt_us = tnow - _state.last_time_micros;
+    
+    // update total current drawn since startup
+    update_consumed(_state, dt_us);
+
+    _state.last_time_micros = tnow;
+}
+
+/*
+ read word from register
+ returns true if read was successful, false if failed
+*/
+bool AP_BattMonitor_INA239::read_word(const uint8_t reg, int16_t& data) const
+{
+    // read the appropriate register from the device
+    if (!dev->read_registers(reg<<2, (uint8_t *)&data, sizeof(data))) {
+        return false;
+    }
+
+    // convert byte order
+    data = int16_t(be16toh(uint16_t(data)));
+
+    return true;
+}
+
+/*
+  write word to a register, byte swapped
+  returns true if write was successful, false if failed
+*/
+bool AP_BattMonitor_INA239::write_word(const uint8_t reg, const uint16_t data) const
+{
+    const uint8_t b[3] { uint8_t(reg<<2), uint8_t(data >> 8), uint8_t(data&0xff) };
+    return dev->transfer(b, sizeof(b), nullptr, 0);
+}
+
+void AP_BattMonitor_INA239::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
+            return;
+        }
+    }
+
+    int16_t bus_voltage, current;
+
+    if (!read_word(REG_BUS_VOLTAGE, bus_voltage) ||
+        !read_word(REG_CURRENT, current)) {
+        failed_reads++;
+        if (failed_reads > 10) {
+            // device has disconnected, we need to reconfigure it
+            configured = false;
+        }
+        return;
+    }
+    failed_reads = 0;
+
+    WITH_SEMAPHORE(accumulate.sem);
+    accumulate.volt_sum += bus_voltage * voltage_LSB;
+    accumulate.current_sum += current * current_LSB;
+    accumulate.count++;
+}
+
+#endif // HAL_BATTMON_INA239_ENABLED

libraries/AP_BattMonitor/AP_BattMonitor_INA239.h

@@ -0,0 +1,54 @@
+#pragma once
+
+#include <AP_Common/AP_Common.h>
+#include <AP_HAL/SPIDevice.h>
+#include "AP_BattMonitor_Backend.h"
+#include <utility>
+
+#ifndef HAL_BATTMON_INA239_ENABLED
+#define HAL_BATTMON_INA239_ENABLED defined(HAL_BATTMON_INA239_SPI_DEVICE)
+#endif
+
+#if HAL_BATTMON_INA239_ENABLED
+
+class AP_BattMonitor_INA239 : public AP_BattMonitor_Backend
+{
+public:
+    /// Constructor
+    AP_BattMonitor_INA239(AP_BattMonitor &mon,
+                          AP_BattMonitor::BattMonitor_State &mon_state,
+                          AP_BattMonitor_Params &params);
+
+    bool has_cell_voltages() const override { return false; }
+    bool has_temperature() const override { return false; }
+    bool has_current() const override { return true; }
+    bool reset_remaining(float percentage) override { return false; }
+    bool get_cycle_count(uint16_t &cycles) const override { return false; }
+
+    void init(void) override;
+    void read() override;
+
+protected:
+    AP_HAL::OwnPtr<AP_HAL::Device> dev;
+
+    void configure(void);
+    bool read_word(const uint8_t reg, int16_t& data) const;
+    bool write_word(const uint8_t reg, const uint16_t data) const;
+    void timer(void);
+
+    bool configured;
+    bool callback_registered;
+    uint32_t failed_reads;
+    uint32_t last_configure_ms;
+
+    struct {
+        uint16_t count;
+        float volt_sum;
+        float current_sum;
+        HAL_Semaphore sem;
+    } accumulate;
+    float current_LSB;
+    float voltage_LSB;
+};
+
+#endif // HAL_BATTMON_INA239_ENABLED