Line data Source code
1 : #include "AP_BattMonitor_config.h"
2 :
3 : #if AP_BATTERY_INA3221_ENABLED
4 :
5 : #include "AP_BattMonitor_INA3221.h"
6 :
7 : #include <AP_HAL/utility/sparse-endian.h>
8 :
9 : #include <AP_HAL/AP_HAL.h>
10 : #include <AP_HAL/I2CDevice.h>
11 : #include <GCS_MAVLink/GCS.h>
12 :
13 : #define INA3221_DEBUGGING 0
14 :
15 : #if INA3221_DEBUGGING
16 : #include <stdio.h>
17 : #define debug(fmt, args ...) do {printf("INA3221: %s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
18 : // #define debug(fmt, args ...) do {gcs().send_text(MAV_SEVERITY_INFO, "INA3221: %s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
19 : #else
20 : #define debug(fmt, args ...)
21 : #endif
22 :
23 : #ifndef HAL_BATTMON_INA3221_BUS
24 : #define HAL_BATTMON_INA3221_BUS 0
25 : #endif
26 :
27 : #ifndef HAL_BATTMON_INA3221_ADDR
28 : #define HAL_BATTMON_INA3221_ADDR 64
29 : #endif
30 :
31 : #ifndef HAL_BATTMON_INA3221_SHUNT_OHMS
32 : #define HAL_BATTMON_INA3221_SHUNT_OHMS 0.001
33 : #endif
34 :
35 : #define HAL_BATTMON_INA3221_CONV_TIME_140US 0b000
36 : #define HAL_BATTMON_INA3221_CONV_TIME_204US 0b001
37 : #define HAL_BATTMON_INA3221_CONV_TIME_332US 0b010
38 : #define HAL_BATTMON_INA3221_CONV_TIME_588US 0b011
39 : #define HAL_BATTMON_INA3221_CONV_TIME_1100US 0b100
40 : #define HAL_BATTMON_INA3221_CONV_TIME_2116US 0b101
41 : #define HAL_BATTMON_INA3221_CONV_TIME_4156US 0b110
42 : #define HAL_BATTMON_INA3221_CONV_TIME_8244US 0b111
43 :
44 : #define HAL_BATTMON_INA3221_AVG_MODE_1 0b000
45 : #define HAL_BATTMON_INA3221_AVG_MODE_4 0b001
46 : #define HAL_BATTMON_INA3221_AVG_MODE_16 0b010
47 : #define HAL_BATTMON_INA3221_AVG_MODE_64 0b011
48 : #define HAL_BATTMON_INA3221_AVG_MODE_128 0b100
49 : #define HAL_BATTMON_INA3221_AVG_MODE_256 0b101
50 : #define HAL_BATTMON_INA3221_AVG_MODE_512 0b110
51 : #define HAL_BATTMON_INA3221_AVG_MODE_1024 0b111
52 :
53 : #ifndef HAL_BATTMON_INA3221_SHUNT_CONV_TIME_SEL
54 : #define HAL_BATTMON_INA3221_SHUNT_CONV_TIME_SEL HAL_BATTMON_INA3221_CONV_TIME_8244US
55 : #endif
56 :
57 : #ifndef HAL_BATTMON_INA3221_BUS_CONV_TIME_SEL
58 : #define HAL_BATTMON_INA3221_BUS_CONV_TIME_SEL HAL_BATTMON_INA3221_CONV_TIME_8244US
59 : #endif
60 :
61 : #ifndef HAL_BATTMON_INA3221_AVG_MODE_SEL
62 : #define HAL_BATTMON_INA3221_AVG_MODE_SEL HAL_BATTMON_INA3221_AVG_MODE_1024
63 : #endif
64 :
65 : struct AP_BattMonitor_INA3221::AddressDriver AP_BattMonitor_INA3221::address_driver[HAL_BATTMON_INA3221_MAX_DEVICES];
66 : uint8_t AP_BattMonitor_INA3221::address_driver_count;
67 :
68 : const AP_Param::GroupInfo AP_BattMonitor_INA3221::var_info[] = {
69 :
70 : // @Param: I2C_BUS
71 : // @DisplayName: Battery monitor I2C bus number
72 : // @Description: Battery monitor I2C bus number
73 : // @Range: 0 3
74 : // @User: Advanced
75 : // @RebootRequired: True
76 : AP_GROUPINFO("I2C_BUS", 22, AP_BattMonitor_INA3221, i2c_bus, HAL_BATTMON_INA3221_BUS),
77 :
78 : // @Param: I2C_ADDR
79 : // @DisplayName: Battery monitor I2C address
80 : // @Description: Battery monitor I2C address. If this is zero then probe list of supported addresses
81 : // @Range: 0 127
82 : // @User: Advanced
83 : // @RebootRequired: True
84 : AP_GROUPINFO("I2C_ADDR", 23, AP_BattMonitor_INA3221, i2c_address, HAL_BATTMON_INA3221_ADDR),
85 :
86 : // @Param: CHANNEL
87 : // @DisplayName: INA3221 channel
88 : // @Description: INA3221 channel to return data for
89 : // @Range: 1 3
90 : // @User: Advanced
91 : // @RebootRequired: True
92 : AP_GROUPINFO("CHANNEL", 24, AP_BattMonitor_INA3221, channel, 1),
93 :
94 : // CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS
95 :
96 : AP_GROUPEND
97 : };
98 :
99 : extern const AP_HAL::HAL &hal;
100 :
101 6 : AP_BattMonitor_INA3221::AP_BattMonitor_INA3221(
102 : AP_BattMonitor &mon,
103 : AP_BattMonitor::BattMonitor_State &mon_state,
104 6 : AP_BattMonitor_Params ¶ms) :
105 6 : AP_BattMonitor_Backend(mon, mon_state, params)
106 : {
107 6 : AP_Param::setup_object_defaults(this, var_info);
108 6 : _state.var_info = var_info;
109 6 : }
110 :
111 563 : bool AP_BattMonitor_INA3221::AddressDriver::read_register(uint8_t addr, uint16_t &ret)
112 : {
113 563 : if (!dev->transfer(&addr, 1, (uint8_t*)&ret, 2)) {
114 3 : return false;
115 : }
116 560 : ret = be16toh(ret);
117 560 : return true;
118 : }
119 :
120 2 : bool AP_BattMonitor_INA3221::AddressDriver::write_register(uint8_t addr, uint16_t val)
121 : {
122 2 : uint8_t buf[3] { addr, uint8_t(val >> 8), uint8_t(val & 0xFF) };
123 :
124 2 : return dev->transfer(buf, sizeof(buf), nullptr, 0);
125 : }
126 :
127 : #define REG_CONFIGURATION 0x00
128 : #define REG_MANUFACTURER_ID 0xFE
129 : #define REG_DIE_ID 0xFF
130 :
131 2 : bool AP_BattMonitor_INA3221::AddressDriver::write_config(void)
132 : {
133 : // update device configuration
134 : union {
135 : struct PACKED {
136 : uint16_t mode : 3;
137 : uint16_t shunt_voltage_conversiontime : 3;
138 : uint16_t bus_voltage_conversiontime : 3;
139 : uint16_t averaging_mode : 3;
140 : uint16_t ch1_enable : 1;
141 : uint16_t ch2_enable : 1;
142 : uint16_t ch3_enable : 1;
143 : uint16_t reset : 1;
144 : } bits;
145 : uint16_t word;
146 2 : } configuration {{
147 : 0b111, // continuous operation
148 : HAL_BATTMON_INA3221_SHUNT_CONV_TIME_SEL,
149 : HAL_BATTMON_INA3221_BUS_CONV_TIME_SEL,
150 : HAL_BATTMON_INA3221_AVG_MODE_SEL,
151 : // dynamically enable channels to not waste time converting unused data
152 2 : (channel_mask & (1 << 1)) != 0, // enable ch1?
153 2 : (channel_mask & (1 << 2)) != 0, // enable ch2?
154 2 : (channel_mask & (1 << 3)) != 0, // enable ch3?
155 : 0b0, // don't reset...
156 2 : }};
157 :
158 2 : if (!write_register(REG_CONFIGURATION, configuration.word)) {
159 0 : return false;
160 : }
161 :
162 2 : dev_channel_mask = channel_mask; // what's actually in the device now
163 :
164 2 : return true;
165 : }
166 :
167 6 : void AP_BattMonitor_INA3221::init()
168 : {
169 6 : uint8_t dev_address = i2c_address.get();
170 6 : uint8_t dev_bus = i2c_bus.get();
171 6 : uint8_t dev_channel = channel.get();
172 :
173 6 : if ((dev_channel < 1) || (dev_channel > 3)) {
174 : debug("INA3221: nonexistent channel");
175 5 : return;
176 : }
177 :
178 : debug("INA3221: probe ch%d@0x%02x on bus %u", dev_channel, dev_address, dev_bus);
179 :
180 : // check to see if we already have the underlying driver reading the bus:
181 6 : for (uint8_t i=0; i<address_driver_count; i++) {
182 2 : AddressDriver *d = &address_driver[i];
183 2 : if (!d->dev) {
184 0 : continue;
185 : }
186 2 : if (d->address != dev_address) {
187 0 : continue;
188 : }
189 2 : if (d->bus != dev_bus) {
190 0 : continue;
191 : }
192 : debug("Reusing INA3221 driver @0x%02x on bus %u", dev_address, dev_bus);
193 2 : address_driver_state = NEW_NOTHROW AddressDriver::StateList;
194 2 : if (address_driver_state == nullptr) {
195 0 : return;
196 : }
197 2 : address_driver_state->channel = dev_channel;
198 2 : address_driver_state->next = d->statelist;
199 2 : d->statelist = address_driver_state;
200 2 : d->channel_mask |= (1 << dev_channel);
201 2 : return;
202 : }
203 :
204 4 : if (address_driver_count == ARRAY_SIZE(address_driver)) {
205 : debug("INA3221: out of address drivers");
206 0 : return;
207 : }
208 :
209 4 : AddressDriver *d = &address_driver[address_driver_count];
210 4 : d->dev = hal.i2c_mgr->get_device_ptr(i2c_bus, i2c_address, 100000, true, 20);
211 4 : if (!d->dev) {
212 0 : return;
213 : }
214 :
215 4 : d->bus = i2c_bus;
216 4 : d->address = i2c_address;
217 :
218 4 : WITH_SEMAPHORE(d->dev->get_semaphore());
219 :
220 : // check manufacturer_id
221 : uint16_t manufacturer_id;
222 4 : if (!d->read_register(REG_MANUFACTURER_ID, manufacturer_id)) {
223 : debug("read register (%u (0x%02x)) failed", REG_MANUFACTURER_ID, REG_MANUFACTURER_ID);
224 3 : return;
225 : }
226 1 : if (manufacturer_id != 0x5449) { // 8.6.1 p24
227 : debug("Bad manufacturer_id: 0x%02x", manufacturer_id);
228 0 : return;
229 : }
230 :
231 : uint16_t die_id;
232 1 : if (!d->read_register(REG_DIE_ID, die_id)) {
233 : debug("Bad die: 0x%02x", manufacturer_id);
234 0 : return;
235 : }
236 1 : if (die_id != 0x3220) { // 8.6.1 p24
237 0 : return;
238 : }
239 :
240 1 : d->channel_mask = (1 << dev_channel);
241 1 : if (!d->write_config()) {
242 0 : return;
243 : }
244 :
245 1 : address_driver_state = NEW_NOTHROW AddressDriver::StateList;
246 1 : if (address_driver_state == nullptr) {
247 0 : return;
248 : }
249 1 : address_driver_state->channel = dev_channel;
250 1 : address_driver_state->next = d->statelist;
251 1 : d->statelist = address_driver_state;
252 :
253 : debug("Found INA3221 ch%d@0x%02x on bus %u", dev_channel, dev_address, dev_bus);
254 :
255 1 : address_driver_count++;
256 :
257 1 : d->register_timer();
258 4 : }
259 :
260 1 : void AP_BattMonitor_INA3221::AddressDriver::register_timer(void)
261 : {
262 1 : dev->register_periodic_callback(
263 : 100000,
264 : FUNCTOR_BIND_MEMBER(&AP_BattMonitor_INA3221::AddressDriver::timer, void));
265 1 : }
266 :
267 94 : void AP_BattMonitor_INA3221::AddressDriver::timer(void)
268 : {
269 94 : bool healthy = true;
270 :
271 94 : if (channel_mask != dev_channel_mask) {
272 1 : if (write_config()) { // update enabled channels
273 1 : return; // data is now out of date, read it next time
274 : }
275 : // continue on to reading if update failed so health gets cleared
276 0 : healthy = false;
277 : }
278 :
279 372 : for (uint8_t i=1; i<=3; i++) {
280 279 : if ((channel_mask & (1U<<i)) == 0) {
281 0 : continue;
282 : }
283 279 : const uint8_t channel_offset = (i-1)*2;
284 279 : const uint8_t reg_shunt = 1 + channel_offset;
285 279 : const uint8_t reg_bus = 2 + channel_offset;
286 :
287 : uint16_t shunt_val;
288 279 : if (!read_register(reg_shunt, shunt_val)) {
289 0 : healthy = false;
290 0 : shunt_val = 0;
291 : }
292 : uint16_t bus_val;
293 279 : if (!read_register(reg_bus, bus_val)) {
294 0 : healthy = false;
295 0 : bus_val = 0;
296 : }
297 :
298 : // 2s complement number with 3 lowest bits not used, 1 count is 8mV
299 279 : const float bus_voltage = ((int16_t)bus_val >> 3)*8e-3;
300 : // 2s complement number with 3 lowest bits not used, 1 count is 40uV
301 279 : const float shunt_voltage = ((int16_t)shunt_val >> 3)*40e-6;
302 279 : const float shunt_resistance = HAL_BATTMON_INA3221_SHUNT_OHMS;
303 279 : const float shunt_current = shunt_voltage/shunt_resistance; // I = V/R
304 :
305 : // transfer readings to state
306 1116 : for (auto *state = statelist; state != nullptr; state = state->next) {
307 837 : if (state->channel != i) {
308 558 : continue;
309 : }
310 279 : WITH_SEMAPHORE(state->sem);
311 :
312 : // calculate time since last data read
313 279 : const uint32_t tnow = AP_HAL::micros();
314 279 : const uint32_t dt_us = tnow - state->last_time_micros;
315 :
316 279 : state->healthy = healthy;
317 279 : state->voltage = bus_voltage;
318 279 : state->current_amps = shunt_current;
319 :
320 : // update current drawn since last reading for read to accumulate
321 279 : if (state->last_time_micros != 0 && dt_us < 2000000) {
322 276 : const float mah = calculate_mah(state->current_amps, dt_us);
323 276 : state->delta_mah += mah;
324 276 : state->delta_wh += 0.001 * mah * state->voltage;
325 : }
326 :
327 279 : state->last_time_micros = tnow;
328 279 : }
329 : }
330 : }
331 :
332 390 : void AP_BattMonitor_INA3221::read()
333 : {
334 390 : if (address_driver_state == nullptr) {
335 195 : return;
336 : }
337 :
338 195 : WITH_SEMAPHORE(address_driver_state->sem);
339 : // copy state data to front-end under semaphore
340 195 : _state.healthy = address_driver_state->healthy;
341 195 : _state.voltage = address_driver_state->voltage;
342 195 : _state.current_amps = address_driver_state->current_amps;
343 195 : _state.consumed_mah += address_driver_state->delta_mah;
344 195 : _state.consumed_wh += address_driver_state->delta_wh;
345 195 : _state.last_time_micros = address_driver_state->last_time_micros;
346 :
347 195 : address_driver_state->delta_mah = 0;
348 195 : address_driver_state->delta_wh = 0;
349 195 : }
350 :
351 : #endif // AP_BATTERY_INA3221_ENABLED
|
