From 5e61e4cdc5d965007fd11389e90172e55a279802 Mon Sep 17 00:00:00 2001 From: Jonathan Loong Date: Wed, 25 Oct 2023 23:22:24 -0700 Subject: [PATCH] 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 | 6 + libraries/AP_BattMonitor/AP_BattMonitor.h | 3 +- .../AP_BattMonitor_AD7091R5.cpp | 236 ++++++++++++++++++ .../AP_BattMonitor/AP_BattMonitor_AD7091R5.h | 75 ++++++ .../AP_BattMonitor/AP_BattMonitor_Params.cpp | 2 +- .../AP_BattMonitor/AP_BattMonitor_config.h | 3 + 6 files changed, 323 insertions(+), 2 deletions(-) create mode 100644 libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.cpp create mode 100644 libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.h diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 5f1ecf8f73..f1d7710bc6 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/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 @@ -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; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index 78311d2541..dbeed2e470 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/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 @@ public: 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); diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.cpp new file mode 100644 index 0000000000..70e37eab43 --- /dev/null +++ b/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 +#include +#include + +//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 ¶ms) : + 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; itransfer(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 diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.h b/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.h new file mode 100644 index 0000000000..e99c7e6620 --- /dev/null +++ b/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 +#include + +#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 ¶ms); + + // 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 _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 diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index f1d6522a6d..12ca57ceed 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/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), diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_config.h b/libraries/AP_BattMonitor/AP_BattMonitor_config.h index 7b14fea17f..4fab7ff189 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_config.h +++ b/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