AP_BattMonitor: added SUI SMBUS battery backend

originally by Kelly Schrock

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -3,6 +3,7 @@
 #include "AP_BattMonitor_SMBus.h"
 #include "AP_BattMonitor_Bebop.h"
 #include "AP_BattMonitor_BLHeliESC.h"
+#include "AP_BattMonitor_SMBus_SUI.h"
 #include "AP_BattMonitor_Sum.h"
 #include "AP_BattMonitor_FuelFlow.h"
 #include "AP_BattMonitor_FuelLevel_PWM.h"
@@ -119,6 +120,18 @@ AP_BattMonitor::init()
                                                                   hal.i2c_mgr->get_device(_params[instance]._i2c_bus, AP_BATTMONITOR_SMBUS_I2C_ADDR,
                                                                                           100000, true, 20));
                 break;
+            case AP_BattMonitor_Params::BattMonitor_TYPE_SUI3:
+                _params[instance]._i2c_bus.set_default(AP_BATTMONITOR_SMBUS_BUS_INTERNAL),
+                drivers[instance] = new AP_BattMonitor_SMBus_SUI(*this, state[instance], _params[instance],
+                                                                 hal.i2c_mgr->get_device(_params[instance]._i2c_bus, AP_BATTMONITOR_SMBUS_I2C_ADDR,
+                                                                                          100000, true, 20), 3);
+                break;
+            case AP_BattMonitor_Params::BattMonitor_TYPE_SUI6:
+                _params[instance]._i2c_bus.set_default(AP_BATTMONITOR_SMBUS_BUS_INTERNAL),
+                drivers[instance] = new AP_BattMonitor_SMBus_SUI(*this, state[instance], _params[instance],
+                                                                 hal.i2c_mgr->get_device(_params[instance]._i2c_bus, AP_BATTMONITOR_SMBUS_I2C_ADDR,
+                                                                                         100000, true, 20), 6);
+                break;
             case AP_BattMonitor_Params::BattMonitor_TYPE_MAXELL:
                 _params[instance]._i2c_bus.set_default(AP_BATTMONITOR_SMBUS_BUS_EXTERNAL);
                 drivers[instance] = new AP_BattMonitor_SMBus_Maxell(*this, state[instance], _params[instance],

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-Maxell,8:UAVCAN-BatteryInfo,9:BLHeli ESC,10:SumOfFollowing,11:FuelFlow,12:FuelLevelPWM
+    // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Maxell,8:UAVCAN-BatteryInfo,9:BLHeli ESC,10:SumOfFollowing,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6
     // @User: Standard
     // @RebootRequired: True
     AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, BattMonitor_TYPE_NONE, AP_PARAM_FLAG_ENABLE),

libraries/AP_BattMonitor/AP_BattMonitor_Params.h

@@ -25,6 +25,8 @@ public:
         BattMonitor_TYPE_Sum                        = 10,
         BattMonitor_TYPE_FuelFlow                   = 11,
         BattMonitor_TYPE_FuelLevel_PWM              = 12,
+        BattMonitor_TYPE_SUI3                       = 13,
+        BattMonitor_TYPE_SUI6                       = 14,
     };
 
     // low voltage sources (used for BATT_LOW_TYPE parameter)

libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp

@@ -16,7 +16,7 @@ AP_BattMonitor_SMBus::AP_BattMonitor_SMBus(AP_BattMonitor &mon,
 void AP_BattMonitor_SMBus::init(void)
 {
     if (_dev) {
-        _dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_SMBus::timer, void));
+        timer_handle = _dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_SMBus::timer, void));
     }
 }
 

libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h

@@ -49,7 +49,7 @@ public:
     // return true if cycle count can be provided and fills in cycles argument
     bool get_cycle_count(uint16_t &cycles) const override;
 
-    void init(void) override;
+    virtual void init(void) override;
 
 protected:
 
@@ -94,6 +94,7 @@ protected:
 
     virtual void timer(void) = 0;   // timer function to read from the battery
 
+    AP_HAL::Device::PeriodicHandle timer_handle;
 };
 
 // include specific implementations

libraries/AP_BattMonitor/AP_BattMonitor_SMBus_SUI.cpp

@@ -0,0 +1,164 @@
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Common/AP_Common.h>
+#include <AP_Math/AP_Math.h>
+#include <AP_Notify/AP_Notify.h>
+#include "AP_BattMonitor.h"
+#include "AP_BattMonitor_SMBus_SUI.h"
+
+extern const AP_HAL::HAL& hal;
+
+#define REG_CELL_VOLTAGE        0x28
+#define REG_CURRENT             0x2a
+
+// maximum number of cells that we can read data for
+#define SUI_MAX_CELL_READ       4
+
+// Constructor
+AP_BattMonitor_SMBus_SUI::AP_BattMonitor_SMBus_SUI(AP_BattMonitor &mon,
+        AP_BattMonitor::BattMonitor_State &mon_state,
+        AP_BattMonitor_Params &params,
+        AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
+        uint8_t _cell_count)
+    : AP_BattMonitor_SMBus(mon, mon_state, params, std::move(dev)),
+      cell_count(_cell_count)
+{
+    _pec_supported = false;
+    _dev->set_retries(2);
+}
+
+void AP_BattMonitor_SMBus_SUI::init(void)
+{
+    AP_BattMonitor_SMBus::init();
+    if (_dev && timer_handle) {
+        // run twice as fast for two phases
+        _dev->adjust_periodic_callback(timer_handle, 50000);
+    }
+}
+
+void AP_BattMonitor_SMBus_SUI::timer()
+{
+    uint32_t tnow = AP_HAL::micros();
+
+    // we read in two phases as the device can stall if you read
+    // current too rapidly after voltages
+    phase_voltages = !phase_voltages;
+
+    if (phase_voltages) {
+        read_cell_voltages();
+        update_health();
+        return;
+    }
+
+    // read current
+    int32_t current_ma;
+    if (read_block_bare(REG_CURRENT, (uint8_t *)&current_ma, sizeof(current_ma))) {
+        _state.current_amps = current_ma * -0.001;
+        _state.last_time_micros = tnow;
+    }
+
+    read_full_charge_capacity();
+
+    read_temp();
+    read_serial_number();
+    read_remaining_capacity();
+    update_health();
+}
+
+// read_block - returns true if successful
+bool AP_BattMonitor_SMBus_SUI::read_block(uint8_t reg, uint8_t* data, uint8_t len) const
+{
+    // buffer to hold results (2 extra byte returned holding length and PEC)
+    uint8_t buff[len+2];
+
+    // read bytes
+    if (!_dev->read_registers(reg, buff, sizeof(buff))) {
+        return false;
+    }
+
+    // get length
+    uint8_t bufflen = buff[0];
+
+    // sanity check length returned by smbus
+    if (bufflen == 0 || bufflen > len) {
+        return false;
+    }
+
+    // check PEC
+    uint8_t pec = get_PEC(AP_BATTMONITOR_SMBUS_I2C_ADDR, reg, true, buff, bufflen+1);
+    if (pec != buff[bufflen+1]) {
+        return false;
+    }
+
+    // copy data (excluding PEC)
+    memcpy(data, &buff[1], bufflen);
+
+    // return success
+    return true;
+}
+
+// read_bare_block - returns true if successful
+bool AP_BattMonitor_SMBus_SUI::read_block_bare(uint8_t reg, uint8_t* data, uint8_t len) const
+{
+    // read bytes
+    if (!_dev->read_registers(reg, data, len)) {
+        return false;
+    }
+
+    // return success
+    return true;
+}
+
+void AP_BattMonitor_SMBus_SUI::read_cell_voltages()
+{
+    // read cell voltages
+    uint16_t voltbuff[SUI_MAX_CELL_READ];
+
+    if (!read_block(REG_CELL_VOLTAGE, (uint8_t *)voltbuff, sizeof(voltbuff))) {
+        return;
+    }
+    float pack_voltage_mv = 0.0f;
+        
+    for (uint8_t i = 0; i < MIN(SUI_MAX_CELL_READ, cell_count); i++) {
+        const uint16_t cell = voltbuff[i];
+        _state.cell_voltages.cells[i] = cell;
+        pack_voltage_mv += (float)cell;
+    }
+
+    if (cell_count >= SUI_MAX_CELL_READ) {
+        // we can't read voltage of all cells. get overall pack voltage to work out
+        // an average for remaining cells
+        uint16_t total_mv;
+        if (read_block(BATTMONITOR_SMBUS_VOLTAGE, (uint8_t *)&total_mv, sizeof(total_mv))) {
+            // if total voltage is below pack_voltage_mv then we will
+            // read zero volts for the extra cells.
+            total_mv = MAX(total_mv, pack_voltage_mv);
+            const uint16_t cell_mv = (total_mv - pack_voltage_mv) / (cell_count - SUI_MAX_CELL_READ);
+            for (uint8_t i = SUI_MAX_CELL_READ; i < cell_count; i++) {
+                _state.cell_voltages.cells[i] = cell_mv;
+            }
+            pack_voltage_mv = total_mv;
+        } else {
+            // we can't get total pack voltage. Use average of cells we have so far
+            for (uint8_t i = SUI_MAX_CELL_READ; i < cell_count; i++) {
+                _state.cell_voltages.cells[i] = pack_voltage_mv / SUI_MAX_CELL_READ;
+                pack_voltage_mv += _state.cell_voltages.cells[i];
+            }
+        }
+    }
+
+    _has_cell_voltages = true;
+
+    // accumulate the pack voltage out of the total of the cells
+    _state.voltage = pack_voltage_mv * 0.001;
+    last_volt_read_us = AP_HAL::micros();
+}
+
+/*
+  update healthy flag
+ */
+void AP_BattMonitor_SMBus_SUI::update_health()
+{
+    uint32_t now = AP_HAL::micros();
+    _state.healthy = (now - last_volt_read_us < AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS) &&
+        (now - _state.last_time_micros < AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS);
+}

libraries/AP_BattMonitor/AP_BattMonitor_SMBus_SUI.h

@@ -0,0 +1,35 @@
+#pragma once
+
+#include <AP_Common/AP_Common.h>
+#include <AP_Math/AP_Math.h>
+#include "AP_BattMonitor_SMBus.h"
+#include <AP_HAL/I2CDevice.h>
+
+// Base SUI class
+class AP_BattMonitor_SMBus_SUI : public AP_BattMonitor_SMBus
+{
+public:
+
+    // Constructor
+    AP_BattMonitor_SMBus_SUI(AP_BattMonitor &mon,
+                             AP_BattMonitor::BattMonitor_State &mon_state,
+                             AP_BattMonitor_Params &params,
+                             AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
+                             uint8_t cell_count
+                            );
+
+    void init(void) override;
+
+private:
+    void timer(void) override;
+    void read_cell_voltages();
+    void update_health();
+
+    // read_block - returns number of characters read if successful, zero if unsuccessful
+    bool read_block(uint8_t reg, uint8_t* data, uint8_t len) const;
+    bool read_block_bare(uint8_t reg, uint8_t* data, uint8_t len) const;
+
+    const uint8_t cell_count;
+    bool phase_voltages;
+    uint32_t last_volt_read_us;
+};