AP_BattMonitor: Addition of AD7091R5 ADC I2C Read Driver

This is an ADC extender based on I2C which is used to read the current and voltage. Enable AD7091R5 in config.h which was reserved previously

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -20,6 +20,7 @@
 #include "AP_BattMonitor_Torqeedo.h"
 #include "AP_BattMonitor_FuelLevel_Analog.h"
 #include "AP_BattMonitor_Synthetic_Current.h"
+#include "AP_BattMonitor_AD7091R5.h"
 
 #include <AP_HAL/AP_HAL.h>
 
@@ -554,6 +555,11 @@ AP_BattMonitor::init()
                 drivers[instance] = new AP_BattMonitor_EFI(*this, state[instance], _params[instance]);
                 break;
 #endif // AP_BATTERY_EFI_ENABLED
+#if AP_BATTERY_AD7091R5_ENABLED
+            case Type::AD7091R5:
+                drivers[instance] = new AP_BattMonitor_AD7091R5(*this, state[instance], _params[instance]);
+                break;
+#endif// AP_BATTERY_AD7091R5_ENABLED
             case Type::NONE:
             default:
                 break;

libraries/AP_BattMonitor/AP_BattMonitor.h

@@ -65,6 +65,7 @@ class AP_BattMonitor
     friend class AP_BattMonitor_INA2XX;
     friend class AP_BattMonitor_INA239;
     friend class AP_BattMonitor_LTC2946;
+    friend class AP_BattMonitor_AD7091R5;
 
     friend class AP_BattMonitor_Torqeedo;
     friend class AP_BattMonitor_FuelLevel_Analog;
@@ -107,7 +108,7 @@ class AP_BattMonitor
         Analog_Volt_Synthetic_Current  = 25,
         INA239_SPI                     = 26,
         EFI                            = 27,
-        // AD7091R5_I2C_Analog         = 28, reserve ID for future use
+        AD7091R5                       = 28,
     };
 
     FUNCTOR_TYPEDEF(battery_failsafe_handler_fn_t, void, const char *, const int8_t);

libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.cpp

@@ -0,0 +1,236 @@
+#include "AP_BattMonitor_AD7091R5.h"
+
+/**
+ * @brief You can use it to Read Current and voltage of 1-3 batteries from a ADC extender IC over I2C.
+ * AD7091R5 is a ADC extender and we are using it to read current and voltage of multiple batteries.
+ * Examples of Pin combination:
+ *  1)Pin 50 = Voltage 51,52,53 =  Current. For 3 battery combination Voltage will be same accross.
+ *  2)Pin 50,51 = Voltage and Current Battery 1 -   Pin 52,53 = Voltage and Current Battery 2
+ * Only the First instance of Battery Monitor will be reading the values from IC over I2C.
+ * Make sure you understand the method of calculation used in this driver before using it.
+ * e.g. using pin 1 on IC to read voltage of 2 batteries and pin 2 and 3 to read current from individual battery.
+ * Pin number represents 50 = pin 1, 51 = pin 2 and so on 52, 53
+ * BATT2_Monitor  = 24 ,  BATT3_Monitor  = 24
+ * BATT2_VOLT_PIN = 50 ,  BATT3_VOLT_PIN = 50
+ * BATT2_CURR_PIN = 51 ,  BATT3_CURR_PIN = 52
+ *
+ *
+ */
+
+#if AP_BATTERY_AD7091R5_ENABLED
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Common/AP_Common.h>
+#include <AP_Math/AP_Math.h>
+
+//macro defines
+#define AD7091R5_I2C_ADDR        0x2F // A0 and A1 tied to GND
+#define AD7091R5_I2C_BUS         0
+#define AD7091R5_RESET           0x02
+#define AD7091R5_RESULT_ADDR     0x00
+#define AD7091R5_CHAN_ADDR       0x01
+#define AD7091R5_CONF_ADDR       0x02
+#define AD7091R5_CH_ID(x)        ((x >> 5) & 0x03)
+#define AD7091R5_RES_MASK        0x0F
+#define AD7091R5_REF             3.3f
+#define AD7091R5_RESOLUTION      (float)4096
+#define AD7091R5_PERIOD_USEC     100000
+#define AD7091R5_BASE_PIN        50
+
+
+extern const AP_HAL::HAL& hal;
+const AP_Param::GroupInfo AP_BattMonitor_AD7091R5::var_info[] = {
+
+    // @Param: VOLT_PIN
+    // @DisplayName: Battery Voltage sensing pin on the AD7091R5 Ic
+    // @Description: Sets the analog input pin that should be used for voltage monitoring on AD7091R5.
+    // @Values: -1:Disabled
+    // @User: Standard
+    // @RebootRequired: True
+    AP_GROUPINFO("VOLT_PIN", 56, AP_BattMonitor_AD7091R5, _volt_pin, 0),
+
+    // @Param: CURR_PIN
+    // @DisplayName: Battery Current sensing pin
+    // @Description: Sets the analog input pin that should be used for Current monitoring on AD7091R5.
+    // @Values: -1:Disabled
+    // @User: Standard
+    // @RebootRequired: True
+    AP_GROUPINFO("CURR_PIN", 57, AP_BattMonitor_AD7091R5, _curr_pin, 0),
+
+    // @Param: VOLT_MULT
+    // @DisplayName: Voltage Multiplier
+    // @Description: Used to convert the voltage of the voltage sensing pin (@PREFIX@VOLT_PIN) to the actual battery's voltage (pin_voltage * VOLT_MULT).
+    // @User: Advanced
+    AP_GROUPINFO("VOLT_MULT", 58, AP_BattMonitor_AD7091R5, _volt_multiplier, 0),
+
+    // @Param: AMP_PERVLT
+    // @DisplayName: Amps per volt
+    // @Description: Number of amps that a 1V reading on the current sensor corresponds to.
+    // @Units: A/V
+    // @User: Standard
+    AP_GROUPINFO("AMP_PERVLT", 59, AP_BattMonitor_AD7091R5, _curr_amp_per_volt, 0),
+
+    // @Param: AMP_OFFSET
+    // @DisplayName: AMP offset
+    // @Description: Voltage offset at zero current on current sensor
+    // @Units: V
+    // @User: Standard
+    AP_GROUPINFO("AMP_OFFSET", 60, AP_BattMonitor_AD7091R5, _curr_amp_offset, 0),
+
+    // @Param: VLT_OFFSET
+    // @DisplayName: Volage offset
+    // @Description: Voltage offset on voltage pin. This allows for an offset due to a diode. This voltage is subtracted before the scaling is applied
+    // @Units: V
+    // @User: Advanced
+    AP_GROUPINFO("VLT_OFFSET", 61, AP_BattMonitor_AD7091R5, _volt_offset, 0),
+
+    // Param indexes must be 56 to 61 to avoid conflict with other battery monitor param tables loaded by pointer
+
+    AP_GROUPEND
+};
+
+
+//Variable initialised to read from first instance.
+AP_BattMonitor_AD7091R5::AnalogData AP_BattMonitor_AD7091R5::_analog_data[AD7091R5_NO_OF_CHANNELS];
+bool AP_BattMonitor_AD7091R5::_first = true;
+bool AP_BattMonitor_AD7091R5::_health = false;
+
+/**
+ * @brief Construct a new ap battmonitor ad7091r5::ap battmonitor ad7091r5 object
+ *
+ * @param mon
+ * @param mon_state
+ * @param params
+ */
+AP_BattMonitor_AD7091R5::AP_BattMonitor_AD7091R5(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;
+}
+
+/**
+ * @brief probe and initialize the sensor and register call back
+ *
+ */
+void AP_BattMonitor_AD7091R5::init()
+{
+    // voltage and current pins from params and check if there are in range
+    if (_volt_pin.get() >= AD7091R5_BASE_PIN && _volt_pin.get() <= AD7091R5_BASE_PIN + AD7091R5_NO_OF_CHANNELS && 
+    _curr_pin.get() >= AD7091R5_BASE_PIN && _curr_pin.get() <= AD7091R5_BASE_PIN + AD7091R5_NO_OF_CHANNELS) {
+        volt_buff_pt = _volt_pin.get() - AD7091R5_BASE_PIN;
+        curr_buff_pt = _curr_pin.get() - AD7091R5_BASE_PIN;
+    }
+    else{
+        return; //pin values are out of range
+    }
+
+    // only the first instance read the i2c device
+    if (_first) {
+        _first = false;
+        // probe i2c device
+        _dev = hal.i2c_mgr->get_device(AD7091R5_I2C_BUS, AD7091R5_I2C_ADDR);
+
+        if (_dev) {
+            WITH_SEMAPHORE(_dev->get_semaphore());
+            _dev->set_retries(10); // lots of retries during probe
+            //Reset and config device
+            if (_initialize()) {
+                _dev->set_retries(2); // drop to 2 retries for runtime
+                _dev->register_periodic_callback(AD7091R5_PERIOD_USEC, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_AD7091R5::_read_adc, void));
+            }
+        }
+    }
+}
+
+/**
+ * @brief read - read the voltage and curren
+ *
+ */
+void AP_BattMonitor_AD7091R5::read()
+{
+
+    WITH_SEMAPHORE(sem);
+    //copy global health status to all instances
+    _state.healthy = _health;
+
+    //return if system not healthy
+    if (!_state.healthy) {
+        return;
+    }
+
+    //voltage conversion
+    _state.voltage = (_data_to_volt(_analog_data[volt_buff_pt].data) - _volt_offset) * _volt_multiplier;
+
+    //current amps conversion
+    _state.current_amps = (_data_to_volt(_analog_data[curr_buff_pt].data) - _curr_amp_offset) * _curr_amp_per_volt;
+
+    // calculate time since last current read
+    uint32_t tnow = AP_HAL::micros();
+    uint32_t dt_us = tnow - _state.last_time_micros;
+
+    // update total current drawn since startup
+    update_consumed(_state, dt_us);
+
+    // record time
+    _state.last_time_micros = tnow;
+}
+
+/**
+ * @brief read all four channels and store the results
+ *
+ */
+void AP_BattMonitor_AD7091R5::_read_adc()
+{
+    uint8_t data[AD7091R5_NO_OF_CHANNELS*2];
+    //reset and reconfigure IC if health status is not good.
+    if (!_state.healthy) {
+        _initialize();
+    }
+    //read value
+    bool ret = _dev->transfer(nullptr, 0, data, sizeof(data));
+    WITH_SEMAPHORE(sem);
+    if (ret) {
+        for (int i=0; i<AD7091R5_NO_OF_CHANNELS; i++) {
+            uint8_t chan = AD7091R5_CH_ID(data[2*i]);
+            _analog_data[chan].data = ((uint16_t)(data[2*i]&AD7091R5_RES_MASK)<<8) | data[2*i+1];
+        }
+        _health = true;
+    } else {
+        _health = false;
+    }
+}
+
+/**
+ * @brief config the adc
+ *
+ * @return true
+ * @return false
+ */
+bool AP_BattMonitor_AD7091R5::_initialize()
+{
+    //reset the device
+    uint8_t data[3] = {AD7091R5_CONF_ADDR, AD7091R5_CONF_CMD | AD7091R5_RESET, AD7091R5_CONF_PDOWN0};
+
+    if(_dev->transfer(data, sizeof(data), nullptr, 0)){
+        //command mode, use external 3.3 reference, all channels enabled, set address pointer register to read the adc results
+        uint8_t data_2[6] = {AD7091R5_CONF_ADDR, AD7091R5_CONF_CMD, AD7091R5_CONF_PDOWN0, AD7091R5_CHAN_ADDR, AD7091R5_CHAN_ALL, AD7091R5_RESULT_ADDR};
+        return _dev->transfer(data_2, sizeof(data_2), nullptr, 0);
+    }
+    return false;
+}
+
+/**
+ * @brief convert binary reading to volts
+ *
+ * @param data
+ * @return float
+ */
+float AP_BattMonitor_AD7091R5::_data_to_volt(uint32_t data)
+{
+    return (AD7091R5_REF/AD7091R5_RESOLUTION)*data;
+}
+
+#endif // AP_BATTERY_AD7091R5_ENABLED

libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.h

@@ -0,0 +1,75 @@
+#pragma once
+
+#include "AP_BattMonitor_Backend.h"
+
+#ifndef AP_BATTERY_AD7091R5_ENABLED
+#define AP_BATTERY_AD7091R5_ENABLED (BOARD_FLASH_SIZE > 1024)
+#endif
+
+#if AP_BATTERY_AD7091R5_ENABLED
+
+#include <AP_HAL/utility/OwnPtr.h>
+#include <AP_HAL/I2CDevice.h>
+
+#define AD7091R5_NO_OF_CHANNELS  4
+#define AD7091R5_CONF_CMD        0x04
+#define AD7091R5_CHAN_ALL        0x0F
+#define AD7091R5_CONF_PDOWN0     0x00
+#define AD7091R5_CONF_PDOWN2     0x02
+#define AD7091R5_CONF_PDOWN3     0x03
+#define AD7091R5_CONF_PDOWN_MASK 0x03
+
+class AP_BattMonitor_AD7091R5 : public AP_BattMonitor_Backend
+{
+public:
+    // Constructor
+    AP_BattMonitor_AD7091R5(AP_BattMonitor &mon,
+                            AP_BattMonitor::BattMonitor_State &mon_state,
+                            AP_BattMonitor_Params &params);
+
+    // Read the battery voltage and current.  Should be called at 10hz
+    void read() override;
+    void init(void) override;
+
+    // returns true if battery monitor provides consumed energy info
+    bool has_consumed_energy() const override
+    {
+        return has_current();
+    }
+
+    // returns true if battery monitor provides current info
+    bool has_current() const override
+    {
+        return true;
+    }
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+private:
+    void _read_adc();
+    bool _initialize();
+    float _data_to_volt(uint32_t data);
+
+    static struct AnalogData {
+        uint32_t data;
+    } _analog_data[AD7091R5_NO_OF_CHANNELS];
+    static bool _first;
+    static bool _health;
+
+    HAL_Semaphore sem; // semaphore for access to shared frontend data
+    AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
+    uint8_t volt_buff_pt;
+    uint8_t curr_buff_pt;
+
+protected:
+
+    // Parameters
+    AP_Float _volt_multiplier;          // voltage on volt pin multiplied by this to calculate battery voltage
+    AP_Float _curr_amp_per_volt;        // voltage on current pin multiplied by this to calculate current in amps
+    AP_Float _curr_amp_offset;          // offset voltage that is subtracted from current pin before conversion to amps
+    AP_Float _volt_offset;              // offset voltage that is subtracted from voltage pin before conversion
+    AP_Int8  _volt_pin;                 // board pin used to measure battery voltage
+    AP_Int8  _curr_pin;                 // board pin used to measure battery current
+};
+
+#endif // AP_BATTERY_AD7091R5_ENABLED

libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp

@@ -17,7 +17,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
+    // @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
     // @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

@@ -86,6 +86,9 @@
 #define AP_BATTERY_TORQEEDO_ENABLED HAL_TORQEEDO_ENABLED
 #endif
 
+#ifndef AP_BATTERY_AD7091R5_ENABLED 
+#define AP_BATTERY_AD7091R5_ENABLED AP_BATTERY_BACKEND_DEFAULT_ENABLED && (BOARD_FLASH_SIZE > 1024)
+#endif
 
 // SMBus-subclass backends:
 #ifndef AP_BATTERY_SMBUS_GENERIC_ENABLED