Line data Source code
1 : #include "AP_BattMonitor_config.h"
2 :
3 : #if AP_BATTERY_INA2XX_ENABLED
4 :
5 : /*
6 : supports INA226, INA228 and INA238 I2C battery monitors
7 : */
8 :
9 : #include <AP_HAL/utility/sparse-endian.h>
10 :
11 : #include "AP_BattMonitor_INA2xx.h"
12 :
13 : extern const AP_HAL::HAL& hal;
14 :
15 :
16 : // INA226 specific registers
17 : #define REG_226_CONFIG 0x00
18 : #define REG_226_CONFIG_DEFAULT 0x4127
19 : #define REG_226_CONFIG_RESET 0x8000
20 : #define REG_226_BUS_VOLTAGE 0x02
21 : #define REG_226_CURRENT 0x04
22 : #define REG_226_CALIBRATION 0x05
23 : #define REG_226_MANUFACT_ID 0xfe
24 :
25 : // INA228 specific registers
26 : #define REG_228_CONFIG 0x00
27 : #define REG_228_CONFIG_RESET 0x8000
28 : #define REG_228_ADC_CONFIG 0x01
29 : #define REG_228_SHUNT_CAL 0x02
30 : #define REG_228_VBUS 0x05
31 : #define REG_228_CURRENT 0x07
32 : #define REG_228_MANUFACT_ID 0x3e
33 : #define REG_228_DEVICE_ID 0x3f
34 : #define REG_228_DIETEMP 0x06
35 : #define INA_228_TEMP_C_LSB 7.8125e-3
36 :
37 : // INA237/INA238 specific registers
38 : #define REG_238_CONFIG 0x00
39 : #define REG_238_CONFIG_RESET 0x8000
40 : #define REG_238_ADC_CONFIG 0x01
41 : #define REG_238_SHUNT_CAL 0x02
42 : #define REG_238_VBUS 0x05
43 : #define REG_238_CURRENT 0x07
44 : #define REG_238_MANUFACT_ID 0x3e
45 : #define REG_238_DEVICE_ID 0x3f
46 : #define REG_238_DIETEMP 0x06
47 : #define INA_238_TEMP_C_LSB 7.8125e-3 // need to mask bottom 4 bits
48 :
49 : // INA231 specific registers
50 : #define REG_231_CONFIG 0x00
51 : #define REG_231_SHUNT_VOLTAGE 0x01
52 : #define REG_231_BUS_VOLTAGE 0x02
53 : #define REG_231_POWER 0x03
54 : #define REG_231_CURRENT 0x04
55 : #define REG_231_CALIBRATION 0x05
56 : #define REG_231_MASK 0x06
57 : #define REG_231_ALERT 0x07
58 :
59 : // INA260 specific registers
60 : #define REG_260_CONFIG 0x00
61 : #define REG_260_CURRENT 0x01
62 : #define REG_260_BUS_VOLTAGE 0x02
63 : #define REG_260_POWER 0x03
64 : #define REG_260_MASK 0x06
65 : #define REG_260_ALERT 0x07
66 : #define REG_260_MANUFACT_ID 0xfe
67 : #define REG_260_DIE_ID 0xff
68 :
69 : #ifndef DEFAULT_BATTMON_INA2XX_MAX_AMPS
70 : #define DEFAULT_BATTMON_INA2XX_MAX_AMPS 90.0
71 : #endif
72 :
73 : #ifndef DEFAULT_BATTMON_INA2XX_SHUNT
74 : #define DEFAULT_BATTMON_INA2XX_SHUNT 0.0005
75 : #endif
76 :
77 : #ifndef HAL_BATTMON_INA2XX_BUS
78 : #define HAL_BATTMON_INA2XX_BUS 0
79 : #endif
80 : #ifndef HAL_BATTMON_INA2XX_ADDR
81 : #define HAL_BATTMON_INA2XX_ADDR 0
82 : #endif
83 :
84 : // list of addresses to probe if I2C_ADDR is zero
85 : const uint8_t AP_BattMonitor_INA2XX::i2c_probe_addresses[] { 0x40, 0x41, 0x44, 0x45 };
86 :
87 : const AP_Param::GroupInfo AP_BattMonitor_INA2XX::var_info[] = {
88 :
89 : // @Param: I2C_BUS
90 : // @DisplayName: Battery monitor I2C bus number
91 : // @Description: Battery monitor I2C bus number
92 : // @Range: 0 3
93 : // @User: Advanced
94 : // @RebootRequired: True
95 : AP_GROUPINFO("I2C_BUS", 25, AP_BattMonitor_INA2XX, i2c_bus, HAL_BATTMON_INA2XX_BUS),
96 :
97 : // @Param: I2C_ADDR
98 : // @DisplayName: Battery monitor I2C address
99 : // @Description: Battery monitor I2C address in decimal. If this is zero then probe list of supported addresses
100 : // @Range: 0 127
101 : // @User: Advanced
102 : // @RebootRequired: True
103 : AP_GROUPINFO("I2C_ADDR", 26, AP_BattMonitor_INA2XX, i2c_address, HAL_BATTMON_INA2XX_ADDR),
104 :
105 : // @Param: MAX_AMPS
106 : // @DisplayName: Battery monitor max current
107 : // @Description: This controls the maximum current the INS2XX sensor will work with.
108 : // @Range: 1 400
109 : // @Units: A
110 : // @User: Advanced
111 : AP_GROUPINFO("MAX_AMPS", 27, AP_BattMonitor_INA2XX, max_amps, DEFAULT_BATTMON_INA2XX_MAX_AMPS),
112 :
113 : // @Param: SHUNT
114 : // @DisplayName: Battery monitor shunt resistor
115 : // @Description: This sets the shunt resistor used in the device
116 : // @Range: 0.0001 0.01
117 : // @Increment: 0.0001
118 : // @Units: Ohm
119 : // @User: Advanced
120 : AP_GROUPINFO("SHUNT", 28, AP_BattMonitor_INA2XX, rShunt, DEFAULT_BATTMON_INA2XX_SHUNT),
121 :
122 : // CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS
123 :
124 : AP_GROUPEND
125 : };
126 :
127 0 : AP_BattMonitor_INA2XX::AP_BattMonitor_INA2XX(AP_BattMonitor &mon,
128 : AP_BattMonitor::BattMonitor_State &mon_state,
129 0 : AP_BattMonitor_Params ¶ms)
130 0 : : AP_BattMonitor_Backend(mon, mon_state, params)
131 : {
132 0 : AP_Param::setup_object_defaults(this, var_info);
133 0 : _state.var_info = var_info;
134 0 : }
135 :
136 0 : void AP_BattMonitor_INA2XX::init(void)
137 : {
138 0 : dev = hal.i2c_mgr->get_device_ptr(i2c_bus, i2c_address, 100000, false, 20);
139 0 : if (!dev) {
140 0 : return;
141 : }
142 : // register now and configure in the timer callbacks
143 0 : dev->register_periodic_callback(25000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_INA2XX::timer, void));
144 : }
145 :
146 0 : bool AP_BattMonitor_INA2XX::configure(DevType dtype)
147 : {
148 0 : switch (dtype) {
149 0 : case DevType::UNKNOWN:
150 0 : return false;
151 :
152 0 : case DevType::INA226: {
153 : // configure for MAX_AMPS
154 0 : const uint16_t conf = (0x2<<9) | (0x5<<6) | (0x5<<3) | 0x7; // 2ms conv time, 16x sampling
155 0 : current_LSB = max_amps / 32768.0;
156 0 : voltage_LSB = 0.00125; // 1.25mV/bit
157 0 : const uint16_t cal = uint16_t(0.00512 / (current_LSB * rShunt));
158 0 : if (write_word(REG_226_CONFIG, REG_226_CONFIG_RESET) && // reset
159 0 : write_word(REG_226_CONFIG, conf) &&
160 0 : write_word(REG_226_CALIBRATION, cal)) {
161 0 : dev_type = dtype;
162 0 : return true;
163 : }
164 0 : break;
165 : }
166 :
167 0 : case DevType::INA228: {
168 : // configure for MAX_AMPS
169 0 : voltage_LSB = 195.3125e-6; // 195.3125 uV/LSB
170 0 : current_LSB = max_amps / (1U<<19);
171 0 : const uint16_t shunt_cal = uint16_t(13107.2e6 * current_LSB * rShunt) & 0x7FFF;
172 0 : if (write_word(REG_228_CONFIG, REG_228_CONFIG_RESET) && // reset
173 0 : write_word(REG_228_CONFIG, 0) &&
174 0 : write_word(REG_228_SHUNT_CAL, shunt_cal)) {
175 0 : dev_type = dtype;
176 0 : return true;
177 : }
178 0 : break;
179 : }
180 :
181 0 : case DevType::INA238: {
182 : // configure for MAX_AMPS
183 0 : voltage_LSB = 3.125e-3; // 3.125mV/LSB
184 0 : current_LSB = max_amps / (1U<<15);
185 0 : const uint16_t shunt_cal = uint16_t(819.2e6 * current_LSB * rShunt) & 0x7FFF;
186 0 : if (write_word(REG_238_CONFIG, REG_238_CONFIG_RESET) && // reset
187 0 : write_word(REG_238_CONFIG, 0) &&
188 0 : write_word(REG_238_SHUNT_CAL, shunt_cal)) {
189 0 : dev_type = dtype;
190 0 : return true;
191 : }
192 0 : break;
193 : }
194 :
195 0 : case DevType::INA231: {
196 : // no configuration needed
197 0 : voltage_LSB = 1.25e-3;
198 0 : current_LSB = max_amps / (1U<<15);
199 0 : const uint16_t cal = 0.00512 / (current_LSB * rShunt);
200 0 : if (write_word(REG_231_CALIBRATION, cal)) {
201 0 : dev_type = dtype;
202 0 : return true;
203 : }
204 0 : break;
205 : }
206 :
207 0 : case DevType::INA260: {
208 : // Reset device
209 0 : if (!write_word(REG_260_CONFIG, 0x8000)) {
210 0 : return false;
211 : }
212 :
213 : // Set longest conversion time and no averaging
214 : // This is 8.244ms for both voltage and current
215 : // So both should have new readings after 16.488ms
216 : // This is a new reading at 60Hz, we read at 40Hz
217 : // Continuous voltage and current measurement
218 0 : if (!write_word(REG_260_CONFIG, 0x01ff)) {
219 0 : return false;
220 : }
221 :
222 : // Configuration OK
223 : // Set (not save) unused shunt and max amps parameters to reflect specs
224 : // 2 milliohms internal shunt
225 0 : rShunt.set_and_default(0.002);
226 :
227 : // Rated for 30 amps for 5 seconds
228 0 : max_amps.set_and_default(30.0);
229 :
230 : // Set detected type
231 0 : dev_type = dtype;
232 0 : return true;
233 : }
234 :
235 : }
236 0 : return false;
237 : }
238 :
239 : /// read the battery_voltage and current, should be called at 10hz
240 0 : void AP_BattMonitor_INA2XX::read(void)
241 : {
242 0 : WITH_SEMAPHORE(accumulate.sem);
243 0 : _state.healthy = accumulate.count > 0;
244 0 : if (!_state.healthy) {
245 0 : return;
246 : }
247 :
248 0 : _state.voltage = accumulate.volt_sum / accumulate.count;
249 0 : _state.current_amps = accumulate.current_sum / accumulate.count;
250 0 : accumulate.volt_sum = 0;
251 0 : accumulate.current_sum = 0;
252 0 : accumulate.count = 0;
253 :
254 0 : const uint32_t tnow = AP_HAL::micros();
255 0 : const uint32_t dt_us = tnow - _state.last_time_micros;
256 :
257 : // update total current drawn since startup
258 0 : update_consumed(_state, dt_us);
259 :
260 0 : _state.last_time_micros = tnow;
261 0 : }
262 :
263 : /*
264 : read 16 bit word from register
265 : returns true if read was successful, false if failed
266 : */
267 0 : bool AP_BattMonitor_INA2XX::read_word16(const uint8_t reg, int16_t& data) const
268 : {
269 : // read the appropriate register from the device
270 0 : if (!dev->read_registers(reg, (uint8_t *)&data, sizeof(data))) {
271 0 : return false;
272 : }
273 :
274 : // convert byte order
275 0 : data = int16_t(be16toh(uint16_t(data)));
276 :
277 0 : return true;
278 : }
279 :
280 : /*
281 : read 24 bit signed value from register
282 : returns true if read was successful, false if failed
283 : */
284 0 : bool AP_BattMonitor_INA2XX::read_word24(const uint8_t reg, int32_t& data) const
285 : {
286 : // read the appropriate register from the device
287 : uint8_t d[3];
288 0 : if (!dev->read_registers(reg, d, sizeof(d))) {
289 0 : return false;
290 : }
291 : // 24 bit 2s complement data. Shift into upper 24 bits of int32_t then divide by 256
292 : // to cope with negative numbers properly
293 0 : data = d[0]<<24 | d[1]<<16 | d[2] << 8;
294 0 : data = data / 256;
295 :
296 0 : return true;
297 : }
298 :
299 : /*
300 : write word to a register, byte swapped
301 : returns true if write was successful, false if failed
302 : */
303 0 : bool AP_BattMonitor_INA2XX::write_word(const uint8_t reg, const uint16_t data) const
304 : {
305 0 : const uint8_t b[3] { reg, uint8_t(data >> 8), uint8_t(data&0xff) };
306 0 : return dev->transfer(b, sizeof(b), nullptr, 0);
307 : }
308 :
309 : /*
310 : detect device type. This may happen well after power on if battery is
311 : not plugged in yet
312 : */
313 0 : bool AP_BattMonitor_INA2XX::detect_device(void)
314 : {
315 0 : uint32_t now = AP_HAL::millis();
316 0 : if (now - last_detect_ms < 200) {
317 : // don't flood the bus
318 0 : return false;
319 : }
320 0 : last_detect_ms = now;
321 : int16_t id;
322 :
323 0 : WITH_SEMAPHORE(dev->get_semaphore());
324 :
325 0 : if (i2c_address.get() == 0) {
326 : // Cycle through probe address list
327 0 : dev->set_address(i2c_probe_addresses[i2c_probe_next]);
328 0 : i2c_probe_next = (i2c_probe_next+1) % sizeof(i2c_probe_addresses);
329 :
330 : } else {
331 : // User provided address
332 0 : dev->set_address(i2c_address.get());
333 : }
334 :
335 0 : if (read_word16(REG_228_MANUFACT_ID, id) && id == 0x5449 &&
336 0 : read_word16(REG_228_DEVICE_ID, id) && (id&0xFFF0) == 0x2280) {
337 0 : has_temp = true;
338 0 : return configure(DevType::INA228);
339 : }
340 0 : if (read_word16(REG_238_MANUFACT_ID, id) && id == 0x5449 &&
341 0 : read_word16(REG_238_DEVICE_ID, id) && (id&0xFFF0) == 0x2380) {
342 0 : has_temp = true;
343 0 : return configure(DevType::INA238);
344 : }
345 0 : if (read_word16(REG_260_MANUFACT_ID, id) && id == 0x5449 &&
346 0 : read_word16(REG_260_DIE_ID, id) && (id&0xFFF0) == 0x2270) {
347 0 : return configure(DevType::INA260);
348 : }
349 0 : if (read_word16(REG_226_MANUFACT_ID, id) && id == 0x5449 &&
350 0 : write_word(REG_226_CONFIG, REG_226_CONFIG_RESET) &&
351 0 : write_word(REG_226_CONFIG, REG_226_CONFIG_DEFAULT) &&
352 0 : read_word16(REG_226_CONFIG, id) &&
353 0 : id == REG_226_CONFIG_DEFAULT) {
354 0 : return configure(DevType::INA226);
355 : }
356 0 : if (read_word16(REG_231_CONFIG, id) && id == 0x4127) {
357 : // no manufacturer ID for 231
358 0 : return configure(DevType::INA231);
359 : }
360 :
361 0 : return false;
362 0 : }
363 :
364 :
365 0 : void AP_BattMonitor_INA2XX::timer(void)
366 : {
367 0 : if (failed_reads > 10) {
368 : // device has disconnected, we need to reconfigure it
369 0 : dev_type = DevType::UNKNOWN;
370 : }
371 :
372 0 : if (dev_type == DevType::UNKNOWN) {
373 0 : if (!detect_device()) {
374 0 : return;
375 : }
376 :
377 : // Reset failed reads after successful detection
378 0 : failed_reads = 0;
379 : }
380 :
381 0 : float voltage = 0, current = 0;
382 :
383 0 : switch (dev_type) {
384 0 : case DevType::UNKNOWN:
385 0 : return;
386 :
387 0 : case DevType::INA226: {
388 : int16_t bus_voltage16, current16;
389 0 : if (!read_word16(REG_226_BUS_VOLTAGE, bus_voltage16) ||
390 0 : !read_word16(REG_226_CURRENT, current16)) {
391 0 : failed_reads++;
392 0 : return;
393 : }
394 0 : voltage = bus_voltage16 * voltage_LSB;
395 0 : current = current16 * current_LSB;
396 0 : break;
397 : }
398 :
399 0 : case DevType::INA228: {
400 : int32_t bus_voltage24, current24;
401 : int16_t temp16;
402 0 : if (!read_word24(REG_228_VBUS, bus_voltage24) ||
403 0 : !read_word24(REG_228_CURRENT, current24) ||
404 0 : !read_word16(REG_228_DIETEMP, temp16)) {
405 0 : failed_reads++;
406 0 : return;
407 : }
408 0 : voltage = (bus_voltage24>>4) * voltage_LSB;
409 0 : current = (current24>>4) * current_LSB;
410 0 : temperature = temp16 * INA_228_TEMP_C_LSB;
411 0 : break;
412 : }
413 :
414 0 : case DevType::INA238: {
415 : int16_t bus_voltage16, current16, temp16;
416 0 : if (!read_word16(REG_238_VBUS, bus_voltage16) ||
417 0 : !read_word16(REG_238_CURRENT, current16) ||
418 0 : !read_word16(REG_238_DIETEMP, temp16)) {
419 0 : failed_reads++;
420 0 : return;
421 : }
422 0 : voltage = bus_voltage16 * voltage_LSB;
423 0 : current = current16 * current_LSB;
424 0 : temperature = (temp16&0xFFF0) * INA_238_TEMP_C_LSB;
425 0 : break;
426 : }
427 :
428 0 : case DevType::INA231: {
429 : int16_t bus_voltage16, current16;
430 0 : if (!read_word16(REG_231_SHUNT_VOLTAGE, bus_voltage16) ||
431 0 : !read_word16(REG_231_CURRENT, current16)) {
432 0 : failed_reads++;
433 0 : return;
434 : }
435 0 : voltage = bus_voltage16 * voltage_LSB;
436 0 : current = current16 * current_LSB;
437 0 : break;
438 : }
439 :
440 0 : case DevType::INA260: {
441 : int16_t current16, bus_voltage16;
442 0 : if (!read_word16(REG_260_BUS_VOLTAGE, bus_voltage16) ||
443 0 : !read_word16(REG_260_CURRENT, current16)) {
444 0 : failed_reads++;
445 0 : return;
446 : }
447 0 : voltage = bus_voltage16 * 0.00125;
448 0 : current = current16 * 0.00125;
449 0 : break;
450 : }
451 : }
452 :
453 0 : failed_reads = 0;
454 :
455 0 : WITH_SEMAPHORE(accumulate.sem);
456 0 : accumulate.volt_sum += voltage;
457 0 : accumulate.current_sum += current;
458 0 : accumulate.count++;
459 : }
460 :
461 : /*
462 : get last temperature
463 : */
464 0 : bool AP_BattMonitor_INA2XX::get_temperature(float &temp) const
465 : {
466 0 : temp = temperature;
467 0 : return has_temp;
468 : }
469 :
470 : #endif // AP_BATTERY_INA2XX_ENABLED
|
