-
Notifications
You must be signed in to change notification settings - Fork 17.9k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge branch 'master' into ardupilot_tutorial
- Loading branch information
Showing
13 changed files
with
392 additions
and
75 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 ¶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; 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 |
Oops, something went wrong.