Line data Source code
1 : #include "AP_BattMonitor_AD7091R5.h"
2 :
3 : /**
4 : * @brief You can use it to Read Current and voltage of 1-3 batteries from a ADC extender IC over I2C.
5 : * AD7091R5 is a ADC extender and we are using it to read current and voltage of multiple batteries.
6 : * Examples of Pin combination:
7 : * 1)Pin 50 = Voltage 51,52,53 = Current. For 3 battery combination Voltage will be same accross.
8 : * 2)Pin 50,51 = Voltage and Current Battery 1 - Pin 52,53 = Voltage and Current Battery 2
9 : * Only the First instance of Battery Monitor will be reading the values from IC over I2C.
10 : * Make sure you understand the method of calculation used in this driver before using it.
11 : * e.g. using pin 1 on IC to read voltage of 2 batteries and pin 2 and 3 to read current from individual battery.
12 : * Pin number represents 50 = pin 1, 51 = pin 2 and so on 52, 53
13 : * BATT2_Monitor = 24 , BATT3_Monitor = 24
14 : * BATT2_VOLT_PIN = 50 , BATT3_VOLT_PIN = 50
15 : * BATT2_CURR_PIN = 51 , BATT3_CURR_PIN = 52
16 : *
17 : *
18 : */
19 :
20 : #if AP_BATTERY_AD7091R5_ENABLED
21 :
22 : #include <AP_HAL/AP_HAL.h>
23 : #include <AP_Common/AP_Common.h>
24 : #include <AP_Math/AP_Math.h>
25 :
26 : //macro defines
27 : #define AD7091R5_I2C_ADDR 0x2F // A0 and A1 tied to GND
28 : #define AD7091R5_I2C_BUS 0
29 : #define AD7091R5_RESET 0x02
30 : #define AD7091R5_RESULT_ADDR 0x00
31 : #define AD7091R5_CHAN_ADDR 0x01
32 : #define AD7091R5_CONF_ADDR 0x02
33 : #define AD7091R5_CH_ID(x) ((x >> 5) & 0x03)
34 : #define AD7091R5_RES_MASK 0x0F
35 : #define AD7091R5_REF 3.3f
36 : #define AD7091R5_RESOLUTION (float)4096
37 : #define AD7091R5_PERIOD_USEC 100000
38 : #define AD7091R5_BASE_PIN 50
39 :
40 :
41 : extern const AP_HAL::HAL& hal;
42 : const AP_Param::GroupInfo AP_BattMonitor_AD7091R5::var_info[] = {
43 :
44 : // @Param: VOLT_PIN
45 : // @DisplayName: Battery Voltage sensing pin on the AD7091R5 Ic
46 : // @Description: Sets the analog input pin that should be used for voltage monitoring on AD7091R5.
47 : // @Values: -1:Disabled
48 : // @Range: -1 127
49 : // @User: Standard
50 : // @RebootRequired: True
51 : AP_GROUPINFO("VOLT_PIN", 56, AP_BattMonitor_AD7091R5, _volt_pin, 0),
52 :
53 : // @Param: CURR_PIN
54 : // @DisplayName: Battery Current sensing pin
55 : // @Description: Sets the analog input pin that should be used for Current monitoring on AD7091R5.
56 : // @Values: -1:Disabled
57 : // @Range: -1 127
58 : // @User: Standard
59 : // @RebootRequired: True
60 : AP_GROUPINFO("CURR_PIN", 57, AP_BattMonitor_AD7091R5, _curr_pin, 0),
61 :
62 : // @Param: VOLT_MULT
63 : // @DisplayName: Voltage Multiplier
64 : // @Description: Used to convert the voltage of the voltage sensing pin (@PREFIX@VOLT_PIN) to the actual battery's voltage (pin_voltage * VOLT_MULT).
65 : // @User: Advanced
66 : AP_GROUPINFO("VOLT_MULT", 58, AP_BattMonitor_AD7091R5, _volt_multiplier, 0),
67 :
68 : // @Param: AMP_PERVLT
69 : // @DisplayName: Amps per volt
70 : // @Description: Number of amps that a 1V reading on the current sensor corresponds to.
71 : // @Units: A/V
72 : // @User: Standard
73 : AP_GROUPINFO("AMP_PERVLT", 59, AP_BattMonitor_AD7091R5, _curr_amp_per_volt, 0),
74 :
75 : // @Param: AMP_OFFSET
76 : // @DisplayName: AMP offset
77 : // @Description: Voltage offset at zero current on current sensor
78 : // @Units: V
79 : // @User: Standard
80 : AP_GROUPINFO("AMP_OFFSET", 60, AP_BattMonitor_AD7091R5, _curr_amp_offset, 0),
81 :
82 : // @Param: VLT_OFFSET
83 : // @DisplayName: Volage offset
84 : // @Description: Voltage offset on voltage pin. This allows for an offset due to a diode. This voltage is subtracted before the scaling is applied
85 : // @Units: V
86 : // @User: Advanced
87 : AP_GROUPINFO("VLT_OFFSET", 61, AP_BattMonitor_AD7091R5, _volt_offset, 0),
88 :
89 : // CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS
90 :
91 : AP_GROUPEND
92 : };
93 :
94 :
95 : //Variable initialised to read from first instance.
96 : AP_BattMonitor_AD7091R5::AnalogData AP_BattMonitor_AD7091R5::_analog_data[AD7091R5_NO_OF_CHANNELS];
97 : bool AP_BattMonitor_AD7091R5::_first = true;
98 : bool AP_BattMonitor_AD7091R5::_health = false;
99 :
100 : /**
101 : * @brief Construct a new ap battmonitor ad7091r5::ap battmonitor ad7091r5 object
102 : *
103 : * @param mon
104 : * @param mon_state
105 : * @param params
106 : */
107 0 : AP_BattMonitor_AD7091R5::AP_BattMonitor_AD7091R5(AP_BattMonitor &mon,
108 : AP_BattMonitor::BattMonitor_State &mon_state,
109 0 : AP_BattMonitor_Params ¶ms) :
110 0 : AP_BattMonitor_Backend(mon, mon_state, params)
111 : {
112 0 : AP_Param::setup_object_defaults(this, var_info);
113 0 : _state.var_info = var_info;
114 0 : }
115 :
116 : /**
117 : * @brief probe and initialize the sensor and register call back
118 : *
119 : */
120 0 : void AP_BattMonitor_AD7091R5::init()
121 : {
122 : // voltage and current pins from params and check if there are in range
123 0 : if (_volt_pin.get() >= AD7091R5_BASE_PIN && _volt_pin.get() <= AD7091R5_BASE_PIN + AD7091R5_NO_OF_CHANNELS &&
124 0 : _curr_pin.get() >= AD7091R5_BASE_PIN && _curr_pin.get() <= AD7091R5_BASE_PIN + AD7091R5_NO_OF_CHANNELS) {
125 0 : volt_buff_pt = _volt_pin.get() - AD7091R5_BASE_PIN;
126 0 : curr_buff_pt = _curr_pin.get() - AD7091R5_BASE_PIN;
127 : }
128 : else{
129 0 : return; //pin values are out of range
130 : }
131 :
132 : // only the first instance read the i2c device
133 0 : if (_first) {
134 0 : _first = false;
135 : // probe i2c device
136 0 : _dev = hal.i2c_mgr->get_device_ptr(AD7091R5_I2C_BUS, AD7091R5_I2C_ADDR);
137 :
138 0 : if (_dev) {
139 0 : WITH_SEMAPHORE(_dev->get_semaphore());
140 0 : _dev->set_retries(10); // lots of retries during probe
141 : //Reset and config device
142 0 : if (_initialize()) {
143 0 : _dev->set_retries(2); // drop to 2 retries for runtime
144 0 : _dev->register_periodic_callback(AD7091R5_PERIOD_USEC, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_AD7091R5::_read_adc, void));
145 : }
146 0 : }
147 : }
148 : }
149 :
150 : /**
151 : * @brief read - read the voltage and curren
152 : *
153 : */
154 0 : void AP_BattMonitor_AD7091R5::read()
155 : {
156 :
157 0 : WITH_SEMAPHORE(sem);
158 : //copy global health status to all instances
159 0 : _state.healthy = _health;
160 :
161 : //return if system not healthy
162 0 : if (!_state.healthy) {
163 0 : return;
164 : }
165 :
166 : //voltage conversion
167 0 : _state.voltage = (_data_to_volt(_analog_data[volt_buff_pt].data) - _volt_offset) * _volt_multiplier;
168 :
169 : //current amps conversion
170 0 : _state.current_amps = (_data_to_volt(_analog_data[curr_buff_pt].data) - _curr_amp_offset) * _curr_amp_per_volt;
171 :
172 : // calculate time since last current read
173 0 : uint32_t tnow = AP_HAL::micros();
174 0 : uint32_t dt_us = tnow - _state.last_time_micros;
175 :
176 : // update total current drawn since startup
177 0 : update_consumed(_state, dt_us);
178 :
179 : // record time
180 0 : _state.last_time_micros = tnow;
181 0 : }
182 :
183 : /**
184 : * @brief read all four channels and store the results
185 : *
186 : */
187 0 : void AP_BattMonitor_AD7091R5::_read_adc()
188 : {
189 : uint8_t data[AD7091R5_NO_OF_CHANNELS*2];
190 : //reset and reconfigure IC if health status is not good.
191 0 : if (!_state.healthy) {
192 0 : _initialize();
193 : }
194 : //read value
195 0 : bool ret = _dev->transfer(nullptr, 0, data, sizeof(data));
196 0 : WITH_SEMAPHORE(sem);
197 0 : if (ret) {
198 0 : for (int i=0; i<AD7091R5_NO_OF_CHANNELS; i++) {
199 0 : uint8_t chan = AD7091R5_CH_ID(data[2*i]);
200 0 : _analog_data[chan].data = ((uint16_t)(data[2*i]&AD7091R5_RES_MASK)<<8) | data[2*i+1];
201 : }
202 0 : _health = true;
203 : } else {
204 0 : _health = false;
205 : }
206 0 : }
207 :
208 : /**
209 : * @brief config the adc
210 : *
211 : * @return true
212 : * @return false
213 : */
214 0 : bool AP_BattMonitor_AD7091R5::_initialize()
215 : {
216 : //reset the device
217 0 : uint8_t data[3] = {AD7091R5_CONF_ADDR, AD7091R5_CONF_CMD | AD7091R5_RESET, AD7091R5_CONF_PDOWN0};
218 :
219 0 : if(_dev->transfer(data, sizeof(data), nullptr, 0)){
220 : //command mode, use external 3.3 reference, all channels enabled, set address pointer register to read the adc results
221 0 : uint8_t data_2[6] = {AD7091R5_CONF_ADDR, AD7091R5_CONF_CMD, AD7091R5_CONF_PDOWN0, AD7091R5_CHAN_ADDR, AD7091R5_CHAN_ALL, AD7091R5_RESULT_ADDR};
222 0 : return _dev->transfer(data_2, sizeof(data_2), nullptr, 0);
223 : }
224 0 : return false;
225 : }
226 :
227 : /**
228 : * @brief convert binary reading to volts
229 : *
230 : * @param data
231 : * @return float
232 : */
233 0 : float AP_BattMonitor_AD7091R5::_data_to_volt(uint32_t data)
234 : {
235 0 : return (AD7091R5_REF/AD7091R5_RESOLUTION)*data;
236 : }
237 :
238 : #endif // AP_BATTERY_AD7091R5_ENABLED
|
