AP_BattMonitor: added INA231 backend

only enabled via hwdef.dat for now

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -14,6 +14,7 @@
 #include "AP_BattMonitor_FuelLevel_PWM.h"
 #include "AP_BattMonitor_Generator.h"
 #include "AP_BattMonitor_MPPT_PacketDigital.h"
+#include "AP_BattMonitor_INA231.h"
 
 #include <AP_HAL/AP_HAL.h>
 
@@ -295,6 +296,11 @@ AP_BattMonitor::init()
                 drivers[instance] = new AP_BattMonitor_MPPT_PacketDigital(*this, state[instance], _params[instance]);
                 break;
 #endif // HAL_MPPT_PACKETDIGITAL_CAN_ENABLE
+#if HAL_BATTMON_INA231_ENABLED
+            case Type::INA231:
+                drivers[instance] = new AP_BattMonitor_INA231(*this, state[instance], _params[instance]);
+                break;
+#endif
             case Type::NONE:
             default:
                 break;

libraries/AP_BattMonitor/AP_BattMonitor.h

@@ -46,6 +46,7 @@ class AP_BattMonitor_SMBus_Rotoye;
 class AP_BattMonitor_UAVCAN;
 class AP_BattMonitor_Generator;
 class AP_BattMonitor_MPPT_PacketDigital;
+class AP_BattMonitor_INA231;
 
 class AP_BattMonitor
 {
@@ -62,6 +63,7 @@ class AP_BattMonitor
     friend class AP_BattMonitor_FuelLevel_PWM;
     friend class AP_BattMonitor_Generator;
     friend class AP_BattMonitor_MPPT_PacketDigital;
+    friend class AP_BattMonitor_INA231;
 
 public:
 
@@ -93,6 +95,7 @@ public:
         GENERATOR_FUEL             = 18,
         Rotoye                     = 19,
         MPPT_PacketDigital         = 20,
+        INA231                     = 21,
     };
 
     FUNCTOR_TYPEDEF(battery_failsafe_handler_fn_t, void, const char *, const int8_t);

libraries/AP_BattMonitor/AP_BattMonitor_INA231.cpp

@@ -0,0 +1,127 @@
+#include "AP_BattMonitor_INA231.h"
+#include <GCS_MAVLink/GCS.h>
+#include <AP_HAL/utility/sparse-endian.h>
+
+#if HAL_BATTMON_INA231_ENABLED
+
+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
+
+// this should become a parameter in future
+#define MAX_AMPS 90.0
+
+void AP_BattMonitor_INA231::init(void)
+{
+    dev = hal.i2c_mgr->get_device(HAL_BATTMON_INA231_BUS, HAL_BATTMON_INA231_ADDR, 100000, false, 20);
+    if (!dev) {
+        return;
+    }
+
+    int16_t config = 0;
+    WITH_SEMAPHORE(dev->get_semaphore());
+    if (!write_word(REG_CONFIG, REG_CONFIG_RESET) ||
+        !write_word(REG_CONFIG, REG_CONFIG_DEFAULT) ||
+        !read_word(REG_CONFIG, config) ||
+        config != REG_CONFIG_DEFAULT) {
+        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "INA231: Failed to find device 0x%04x", unsigned(config));
+        return;
+    }
+
+    // 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)) {
+        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "INA231: Failed to configure device");
+        return;
+    }
+
+
+    GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "INA231: found monitor on bus %u", HAL_BATTMON_INA231_BUS);
+
+    if (dev) {
+        dev->register_periodic_callback(25000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_INA231::timer, void));
+    }
+}
+
+/// read the battery_voltage and current, should be called at 10hz
+void AP_BattMonitor_INA231::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 float dt = tnow - _state.last_time_micros;
+    
+    // update total current drawn since startup
+    if (_state.last_time_micros != 0 && dt < 2000000.0) {
+        // .0002778 is 1/3600 (conversion to hours)
+        const float mah = _state.current_amps * dt * 0.0000002778;
+        _state.consumed_mah += mah;
+        _state.consumed_wh  += 0.001 * mah * _state.voltage;
+    }
+    _state.last_time_micros = tnow;
+}
+
+/*
+ read word from register
+ returns true if read was successful, false if failed
+*/
+bool AP_BattMonitor_INA231::read_word(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))) {
+        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_INA231::write_word(const uint8_t reg, const uint16_t data) const
+{
+    const uint8_t b[3] { reg, uint8_t(data >> 8), uint8_t(data&0xff) };
+    return dev->transfer(b, sizeof(b), nullptr, 0);
+}
+
+void AP_BattMonitor_INA231::timer(void)
+{
+    int16_t bus_voltage, current;
+    if (!read_word(REG_BUS_VOLTAGE, bus_voltage) ||
+        !read_word(REG_CURRENT, current)) {
+        return;
+    }
+    WITH_SEMAPHORE(accumulate.sem);
+    accumulate.volt_sum += bus_voltage * voltage_LSB;
+    accumulate.current_sum += current * current_LSB;
+    accumulate.count++;
+}
+
+#endif // HAL_BATTMON_INA231_ENABLED

libraries/AP_BattMonitor/AP_BattMonitor_INA231.h

@@ -0,0 +1,44 @@
+#pragma once
+
+#include <AP_Common/AP_Common.h>
+#include <AP_HAL/I2CDevice.h>
+#include "AP_BattMonitor_Backend.h"
+#include <utility>
+
+#define HAL_BATTMON_INA231_ENABLED defined(HAL_BATTMON_INA231_BUS) && defined(HAL_BATTMON_INA231_ADDR)
+
+#if HAL_BATTMON_INA231_ENABLED
+
+class AP_BattMonitor_INA231 : public AP_BattMonitor_Backend
+{
+public:
+    // inherit constructor
+    using AP_BattMonitor_Backend::AP_BattMonitor_Backend;
+
+    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; }
+
+    virtual void init(void) override;
+    virtual void read() override;
+    
+private:
+    AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev;
+
+    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);
+
+    struct {
+        uint16_t count;
+        float volt_sum;
+        float current_sum;
+        HAL_Semaphore sem;
+    } accumulate;
+    float current_LSB;
+    float voltage_LSB;
+};
+
+#endif // HAL_BATTMON_INA231_ENABLED

libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp

@@ -13,7 +13,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:UAVCAN-BatteryInfo,9:ESC,10:SumOfFollowing,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell,17:Generator-Elec,18:Generator-Fuel,19:Rotoye
+    // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Generic,8:UAVCAN-BatteryInfo,9:ESC,10:SumOfFollowing,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:INA231
     // @User: Standard
     // @RebootRequired: True
     AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, int8_t(AP_BattMonitor::Type::NONE), AP_PARAM_FLAG_ENABLE),