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
This commit is contained in:
Jonathan Loong 2023-10-25 23:22:24 -07:00 committed by Andrew Tridgell
parent 8d03d7e15d
commit 5e61e4cdc5
6 changed files with 323 additions and 2 deletions

View File

@ -20,6 +20,7 @@
#include "AP_BattMonitor_Torqeedo.h" #include "AP_BattMonitor_Torqeedo.h"
#include "AP_BattMonitor_FuelLevel_Analog.h" #include "AP_BattMonitor_FuelLevel_Analog.h"
#include "AP_BattMonitor_Synthetic_Current.h" #include "AP_BattMonitor_Synthetic_Current.h"
#include "AP_BattMonitor_AD7091R5.h"
#include <AP_HAL/AP_HAL.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]); drivers[instance] = new AP_BattMonitor_EFI(*this, state[instance], _params[instance]);
break; break;
#endif // AP_BATTERY_EFI_ENABLED #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: case Type::NONE:
default: default:
break; break;

View File

@ -65,6 +65,7 @@ class AP_BattMonitor
friend class AP_BattMonitor_INA2XX; friend class AP_BattMonitor_INA2XX;
friend class AP_BattMonitor_INA239; friend class AP_BattMonitor_INA239;
friend class AP_BattMonitor_LTC2946; friend class AP_BattMonitor_LTC2946;
friend class AP_BattMonitor_AD7091R5;
friend class AP_BattMonitor_Torqeedo; friend class AP_BattMonitor_Torqeedo;
friend class AP_BattMonitor_FuelLevel_Analog; friend class AP_BattMonitor_FuelLevel_Analog;
@ -107,7 +108,7 @@ public:
Analog_Volt_Synthetic_Current = 25, Analog_Volt_Synthetic_Current = 25,
INA239_SPI = 26, INA239_SPI = 26,
EFI = 27, 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); FUNCTOR_TYPEDEF(battery_failsafe_handler_fn_t, void, const char *, const int8_t);

View File

@ -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

View File

@ -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

View File

@ -17,7 +17,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = {
// @Param: MONITOR // @Param: MONITOR
// @DisplayName: Battery monitoring // @DisplayName: Battery monitoring
// @Description: Controls enabling monitoring of the battery's voltage and current // @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 // @User: Standard
// @RebootRequired: True // @RebootRequired: True
AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, int8_t(AP_BattMonitor::Type::NONE), AP_PARAM_FLAG_ENABLE), AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, int8_t(AP_BattMonitor::Type::NONE), AP_PARAM_FLAG_ENABLE),

View File

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