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