Line data Source code
1 : #include "AP_BattMonitor_config.h"
2 :
3 : #if AP_BATTERY_ENABLED
4 :
5 : #include "AP_BattMonitor.h"
6 :
7 : #include "AP_BattMonitor_Analog.h"
8 : #include "AP_BattMonitor_SMBus.h"
9 : #include "AP_BattMonitor_SMBus_Solo.h"
10 : #include "AP_BattMonitor_SMBus_Generic.h"
11 : #include "AP_BattMonitor_SMBus_Maxell.h"
12 : #include "AP_BattMonitor_SMBus_Rotoye.h"
13 : #include "AP_BattMonitor_Bebop.h"
14 : #include "AP_BattMonitor_ESC.h"
15 : #include "AP_BattMonitor_SMBus_SUI.h"
16 : #include "AP_BattMonitor_SMBus_NeoDesign.h"
17 : #include "AP_BattMonitor_Sum.h"
18 : #include "AP_BattMonitor_FuelFlow.h"
19 : #include "AP_BattMonitor_FuelLevel_PWM.h"
20 : #include "AP_BattMonitor_Generator.h"
21 : #include "AP_BattMonitor_EFI.h"
22 : #include "AP_BattMonitor_INA2xx.h"
23 : #include "AP_BattMonitor_INA239.h"
24 : #include "AP_BattMonitor_INA3221.h"
25 : #include "AP_BattMonitor_LTC2946.h"
26 : #include "AP_BattMonitor_Torqeedo.h"
27 : #include "AP_BattMonitor_FuelLevel_Analog.h"
28 : #include "AP_BattMonitor_Synthetic_Current.h"
29 : #include "AP_BattMonitor_AD7091R5.h"
30 : #include "AP_BattMonitor_Scripting.h"
31 : #include "AP_BattMonitor_TIBQ76952.h"
32 :
33 : #include <AP_HAL/AP_HAL.h>
34 :
35 : #if HAL_ENABLE_DRONECAN_DRIVERS
36 : #include "AP_BattMonitor_DroneCAN.h"
37 : #endif
38 :
39 : #include <AP_Vehicle/AP_Vehicle_Type.h>
40 : #include <AP_Logger/AP_Logger.h>
41 : #include <GCS_MAVLink/GCS.h>
42 : #include <AP_Notify/AP_Notify.h>
43 :
44 : extern const AP_HAL::HAL& hal;
45 :
46 : AP_BattMonitor *AP_BattMonitor::_singleton;
47 :
48 : const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
49 : // 0 - 18, 20- 22 used by old parameter indexes
50 :
51 : // Monitor 1
52 :
53 : // @Group: _
54 : // @Path: AP_BattMonitor_Params.cpp
55 : AP_SUBGROUPINFO(_params[0], "_", 23, AP_BattMonitor, AP_BattMonitor_Params),
56 :
57 : // @Group: _
58 : // @Path: AP_BattMonitor_Analog.cpp
59 : // @Group: _
60 : // @Path: AP_BattMonitor_SMBus.cpp
61 : // @Group: _
62 : // @Path: AP_BattMonitor_Sum.cpp
63 : // @Group: _
64 : // @Path: AP_BattMonitor_DroneCAN.cpp
65 : // @Group: _
66 : // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
67 : // @Group: _
68 : // @Path: AP_BattMonitor_Synthetic_Current.cpp
69 : // @Group: _
70 : // @Path: AP_BattMonitor_INA2xx.cpp
71 : // @Group: _
72 : // @Path: AP_BattMonitor_ESC.cpp
73 : // @Group: _
74 : // @Path: AP_BattMonitor_INA239.cpp
75 : // @Group: _
76 : // @Path: AP_BattMonitor_INA3221.cpp
77 : // @Group: _
78 : // @Path: AP_BattMonitor_AD7091R5.cpp
79 : // @Group: _
80 : // @Path: AP_BattMonitor_TIBQ76952.cpp
81 : AP_SUBGROUPVARPTR(drivers[0], "_", 41, AP_BattMonitor, backend_var_info[0]),
82 :
83 : #if AP_BATT_MONITOR_MAX_INSTANCES > 1
84 : // @Group: 2_
85 : // @Path: AP_BattMonitor_Params.cpp
86 : AP_SUBGROUPINFO(_params[1], "2_", 24, AP_BattMonitor, AP_BattMonitor_Params),
87 :
88 : // @Group: 2_
89 : // @Path: AP_BattMonitor_Analog.cpp
90 : // @Group: 2_
91 : // @Path: AP_BattMonitor_SMBus.cpp
92 : // @Group: 2_
93 : // @Path: AP_BattMonitor_Sum.cpp
94 : // @Group: 2_
95 : // @Path: AP_BattMonitor_DroneCAN.cpp
96 : // @Group: 2_
97 : // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
98 : // @Group: 2_
99 : // @Path: AP_BattMonitor_Synthetic_Current.cpp
100 : // @Group: 2_
101 : // @Path: AP_BattMonitor_INA2xx.cpp
102 : // @Group: 2_
103 : // @Path: AP_BattMonitor_ESC.cpp
104 : // @Group: 2_
105 : // @Path: AP_BattMonitor_INA239.cpp
106 : // @Group: 2_
107 : // @Path: AP_BattMonitor_INA3221.cpp
108 : // @Group: 2_
109 : // @Path: AP_BattMonitor_AD7091R5.cpp
110 : // @Group: 2_
111 : // @Path: AP_BattMonitor_TIBQ76952.cpp
112 : AP_SUBGROUPVARPTR(drivers[1], "2_", 42, AP_BattMonitor, backend_var_info[1]),
113 : #endif
114 :
115 : #if AP_BATT_MONITOR_MAX_INSTANCES > 2
116 : // @Group: 3_
117 : // @Path: AP_BattMonitor_Params.cpp
118 : AP_SUBGROUPINFO(_params[2], "3_", 25, AP_BattMonitor, AP_BattMonitor_Params),
119 :
120 : // @Group: 3_
121 : // @Path: AP_BattMonitor_Analog.cpp
122 : // @Group: 3_
123 : // @Path: AP_BattMonitor_SMBus.cpp
124 : // @Group: 3_
125 : // @Path: AP_BattMonitor_Sum.cpp
126 : // @Group: 3_
127 : // @Path: AP_BattMonitor_DroneCAN.cpp
128 : // @Group: 3_
129 : // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
130 : // @Group: 3_
131 : // @Path: AP_BattMonitor_Synthetic_Current.cpp
132 : // @Group: 3_
133 : // @Path: AP_BattMonitor_INA2xx.cpp
134 : // @Group: 3_
135 : // @Path: AP_BattMonitor_ESC.cpp
136 : // @Group: 3_
137 : // @Path: AP_BattMonitor_INA239.cpp
138 : // @Group: 3_
139 : // @Path: AP_BattMonitor_INA3221.cpp
140 : // @Group: 3_
141 : // @Path: AP_BattMonitor_AD7091R5.cpp
142 : // @Group: 3_
143 : // @Path: AP_BattMonitor_TIBQ76952.cpp
144 : AP_SUBGROUPVARPTR(drivers[2], "3_", 43, AP_BattMonitor, backend_var_info[2]),
145 : #endif
146 :
147 : #if AP_BATT_MONITOR_MAX_INSTANCES > 3
148 : // @Group: 4_
149 : // @Path: AP_BattMonitor_Params.cpp
150 : AP_SUBGROUPINFO(_params[3], "4_", 26, AP_BattMonitor, AP_BattMonitor_Params),
151 :
152 : // @Group: 4_
153 : // @Path: AP_BattMonitor_Analog.cpp
154 : // @Group: 4_
155 : // @Path: AP_BattMonitor_SMBus.cpp
156 : // @Group: 4_
157 : // @Path: AP_BattMonitor_Sum.cpp
158 : // @Group: 4_
159 : // @Path: AP_BattMonitor_DroneCAN.cpp
160 : // @Group: 4_
161 : // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
162 : // @Group: 4_
163 : // @Path: AP_BattMonitor_Synthetic_Current.cpp
164 : // @Group: 4_
165 : // @Path: AP_BattMonitor_INA2xx.cpp
166 : // @Group: 4_
167 : // @Path: AP_BattMonitor_ESC.cpp
168 : // @Group: 4_
169 : // @Path: AP_BattMonitor_INA239.cpp
170 : // @Group: 4_
171 : // @Path: AP_BattMonitor_INA3221.cpp
172 : // @Group: 4_
173 : // @Path: AP_BattMonitor_AD7091R5.cpp
174 : // @Group: 4_
175 : // @Path: AP_BattMonitor_TIBQ76952.cpp
176 : AP_SUBGROUPVARPTR(drivers[3], "4_", 44, AP_BattMonitor, backend_var_info[3]),
177 : #endif
178 :
179 : #if AP_BATT_MONITOR_MAX_INSTANCES > 4
180 : // @Group: 5_
181 : // @Path: AP_BattMonitor_Params.cpp
182 : AP_SUBGROUPINFO(_params[4], "5_", 27, AP_BattMonitor, AP_BattMonitor_Params),
183 :
184 : // @Group: 5_
185 : // @Path: AP_BattMonitor_Analog.cpp
186 : // @Group: 5_
187 : // @Path: AP_BattMonitor_SMBus.cpp
188 : // @Group: 5_
189 : // @Path: AP_BattMonitor_Sum.cpp
190 : // @Group: 5_
191 : // @Path: AP_BattMonitor_DroneCAN.cpp
192 : // @Group: 5_
193 : // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
194 : // @Group: 5_
195 : // @Path: AP_BattMonitor_Synthetic_Current.cpp
196 : // @Group: 5_
197 : // @Path: AP_BattMonitor_INA2xx.cpp
198 : // @Group: 5_
199 : // @Path: AP_BattMonitor_ESC.cpp
200 : // @Group: 5_
201 : // @Path: AP_BattMonitor_INA239.cpp
202 : // @Group: 5_
203 : // @Path: AP_BattMonitor_INA3221.cpp
204 : // @Group: 5_
205 : // @Path: AP_BattMonitor_AD7091R5.cpp
206 : // @Group: 5_
207 : // @Path: AP_BattMonitor_TIBQ76952.cpp
208 : AP_SUBGROUPVARPTR(drivers[4], "5_", 45, AP_BattMonitor, backend_var_info[4]),
209 : #endif
210 :
211 : #if AP_BATT_MONITOR_MAX_INSTANCES > 5
212 : // @Group: 6_
213 : // @Path: AP_BattMonitor_Params.cpp
214 : AP_SUBGROUPINFO(_params[5], "6_", 28, AP_BattMonitor, AP_BattMonitor_Params),
215 :
216 : // @Group: 6_
217 : // @Path: AP_BattMonitor_Analog.cpp
218 : // @Group: 6_
219 : // @Path: AP_BattMonitor_SMBus.cpp
220 : // @Group: 6_
221 : // @Path: AP_BattMonitor_Sum.cpp
222 : // @Group: 6_
223 : // @Path: AP_BattMonitor_DroneCAN.cpp
224 : // @Group: 6_
225 : // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
226 : // @Group: 6_
227 : // @Path: AP_BattMonitor_Synthetic_Current.cpp
228 : // @Group: 6_
229 : // @Path: AP_BattMonitor_INA2xx.cpp
230 : // @Group: 6_
231 : // @Path: AP_BattMonitor_ESC.cpp
232 : // @Group: 6_
233 : // @Path: AP_BattMonitor_INA239.cpp
234 : // @Group: 6_
235 : // @Path: AP_BattMonitor_INA3221.cpp
236 : // @Group: 6_
237 : // @Path: AP_BattMonitor_AD7091R5.cpp
238 : // @Group: 6_
239 : // @Path: AP_BattMonitor_TIBQ76952.cpp
240 : AP_SUBGROUPVARPTR(drivers[5], "6_", 46, AP_BattMonitor, backend_var_info[5]),
241 : #endif
242 :
243 : #if AP_BATT_MONITOR_MAX_INSTANCES > 6
244 : // @Group: 7_
245 : // @Path: AP_BattMonitor_Params.cpp
246 : AP_SUBGROUPINFO(_params[6], "7_", 29, AP_BattMonitor, AP_BattMonitor_Params),
247 :
248 : // @Group: 7_
249 : // @Path: AP_BattMonitor_Analog.cpp
250 : // @Group: 7_
251 : // @Path: AP_BattMonitor_SMBus.cpp
252 : // @Group: 7_
253 : // @Path: AP_BattMonitor_Sum.cpp
254 : // @Group: 7_
255 : // @Path: AP_BattMonitor_DroneCAN.cpp
256 : // @Group: 7_
257 : // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
258 : // @Group: 7_
259 : // @Path: AP_BattMonitor_Synthetic_Current.cpp
260 : // @Group: 7_
261 : // @Path: AP_BattMonitor_INA2xx.cpp
262 : // @Group: 7_
263 : // @Path: AP_BattMonitor_ESC.cpp
264 : // @Group: 7_
265 : // @Path: AP_BattMonitor_INA239.cpp
266 : // @Group: 7_
267 : // @Path: AP_BattMonitor_INA3221.cpp
268 : // @Group: 7_
269 : // @Path: AP_BattMonitor_AD7091R5.cpp
270 : // @Group: 7_
271 : // @Path: AP_BattMonitor_TIBQ76952.cpp
272 : AP_SUBGROUPVARPTR(drivers[6], "7_", 47, AP_BattMonitor, backend_var_info[6]),
273 : #endif
274 :
275 : #if AP_BATT_MONITOR_MAX_INSTANCES > 7
276 : // @Group: 8_
277 : // @Path: AP_BattMonitor_Params.cpp
278 : AP_SUBGROUPINFO(_params[7], "8_", 30, AP_BattMonitor, AP_BattMonitor_Params),
279 :
280 : // @Group: 8_
281 : // @Path: AP_BattMonitor_Analog.cpp
282 : // @Group: 8_
283 : // @Path: AP_BattMonitor_SMBus.cpp
284 : // @Group: 8_
285 : // @Path: AP_BattMonitor_Sum.cpp
286 : // @Group: 8_
287 : // @Path: AP_BattMonitor_DroneCAN.cpp
288 : // @Group: 8_
289 : // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
290 : // @Group: 8_
291 : // @Path: AP_BattMonitor_Synthetic_Current.cpp
292 : // @Group: 8_
293 : // @Path: AP_BattMonitor_INA2xx.cpp
294 : // @Group: 8_
295 : // @Path: AP_BattMonitor_ESC.cpp
296 : // @Group: 8_
297 : // @Path: AP_BattMonitor_INA239.cpp
298 : // @Group: 8_
299 : // @Path: AP_BattMonitor_INA3221.cpp
300 : // @Group: 8_
301 : // @Path: AP_BattMonitor_AD7091R5.cpp
302 : // @Group: 8_
303 : // @Path: AP_BattMonitor_TIBQ76952.cpp
304 : AP_SUBGROUPVARPTR(drivers[7], "8_", 48, AP_BattMonitor, backend_var_info[7]),
305 : #endif
306 :
307 : #if AP_BATT_MONITOR_MAX_INSTANCES > 8
308 : // @Group: 9_
309 : // @Path: AP_BattMonitor_Params.cpp
310 : AP_SUBGROUPINFO(_params[8], "9_", 31, AP_BattMonitor, AP_BattMonitor_Params),
311 :
312 : // @Group: 9_
313 : // @Path: AP_BattMonitor_Analog.cpp
314 : // @Group: 9_
315 : // @Path: AP_BattMonitor_SMBus.cpp
316 : // @Group: 9_
317 : // @Path: AP_BattMonitor_Sum.cpp
318 : // @Group: 9_
319 : // @Path: AP_BattMonitor_DroneCAN.cpp
320 : // @Group: 9_
321 : // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
322 : // @Group: 9_
323 : // @Path: AP_BattMonitor_Synthetic_Current.cpp
324 : // @Group: 9_
325 : // @Path: AP_BattMonitor_INA2xx.cpp
326 : // @Group: 9_
327 : // @Path: AP_BattMonitor_ESC.cpp
328 : // @Group: 9_
329 : // @Path: AP_BattMonitor_INA239.cpp
330 : // @Group: 9_
331 : // @Path: AP_BattMonitor_INA3221.cpp
332 : // @Group: 9_
333 : // @Path: AP_BattMonitor_AD7091R5.cpp
334 : // @Group: 9_
335 : // @Path: AP_BattMonitor_TIBQ76952.cpp
336 : AP_SUBGROUPVARPTR(drivers[8], "9_", 49, AP_BattMonitor, backend_var_info[8]),
337 : #endif
338 :
339 : #if AP_BATT_MONITOR_MAX_INSTANCES > 9
340 : // @Group: A_
341 : // @Path: AP_BattMonitor_Params.cpp
342 : AP_SUBGROUPINFO(_params[9], "A_", 32, AP_BattMonitor, AP_BattMonitor_Params),
343 :
344 : // @Group: A_
345 : // @Path: AP_BattMonitor_Analog.cpp
346 : // @Group: A_
347 : // @Path: AP_BattMonitor_SMBus.cpp
348 : // @Group: A_
349 : // @Path: AP_BattMonitor_Sum.cpp
350 : // @Group: A_
351 : // @Path: AP_BattMonitor_DroneCAN.cpp
352 : // @Group: A_
353 : // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
354 : // @Group: A_
355 : // @Path: AP_BattMonitor_Synthetic_Current.cpp
356 : // @Group: A_
357 : // @Path: AP_BattMonitor_INA2xx.cpp
358 : // @Group: A_
359 : // @Path: AP_BattMonitor_ESC.cpp
360 : // @Group: A_
361 : // @Path: AP_BattMonitor_INA239.cpp
362 : // @Group: A_
363 : // @Path: AP_BattMonitor_INA3221.cpp
364 : // @Group: A_
365 : // @Path: AP_BattMonitor_AD7091R5.cpp
366 : // @Group: A_
367 : // @Path: AP_BattMonitor_TIBQ76952.cpp
368 : AP_SUBGROUPVARPTR(drivers[9], "A_", 50, AP_BattMonitor, backend_var_info[9]),
369 : #endif
370 :
371 : #if AP_BATT_MONITOR_MAX_INSTANCES > 10
372 : // @Group: B_
373 : // @Path: AP_BattMonitor_Params.cpp
374 : AP_SUBGROUPINFO(_params[10], "B_", 33, AP_BattMonitor, AP_BattMonitor_Params),
375 :
376 : // @Group: B_
377 : // @Path: AP_BattMonitor_Analog.cpp
378 : // @Group: B_
379 : // @Path: AP_BattMonitor_SMBus.cpp
380 : // @Group: B_
381 : // @Path: AP_BattMonitor_Sum.cpp
382 : // @Group: B_
383 : // @Path: AP_BattMonitor_DroneCAN.cpp
384 : // @Group: B_
385 : // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
386 : // @Group: B_
387 : // @Path: AP_BattMonitor_Synthetic_Current.cpp
388 : // @Group: B_
389 : // @Path: AP_BattMonitor_INA2xx.cpp
390 : // @Group: B_
391 : // @Path: AP_BattMonitor_ESC.cpp
392 : // @Group: B_
393 : // @Path: AP_BattMonitor_INA239.cpp
394 : // @Group: B_
395 : // @Path: AP_BattMonitor_INA3221.cpp
396 : // @Group: B_
397 : // @Path: AP_BattMonitor_AD7091R5.cpp
398 : // @Group: B_
399 : // @Path: AP_BattMonitor_TIBQ76952.cpp
400 : AP_SUBGROUPVARPTR(drivers[10], "B_", 51, AP_BattMonitor, backend_var_info[10]),
401 : #endif
402 :
403 : #if AP_BATT_MONITOR_MAX_INSTANCES > 11
404 : // @Group: C_
405 : // @Path: AP_BattMonitor_Params.cpp
406 : AP_SUBGROUPINFO(_params[11], "C_", 34, AP_BattMonitor, AP_BattMonitor_Params),
407 :
408 : // @Group: C_
409 : // @Path: AP_BattMonitor_Analog.cpp
410 : // @Group: C_
411 : // @Path: AP_BattMonitor_SMBus.cpp
412 : // @Group: C_
413 : // @Path: AP_BattMonitor_Sum.cpp
414 : // @Group: C_
415 : // @Path: AP_BattMonitor_DroneCAN.cpp
416 : // @Group: C_
417 : // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
418 : // @Group: C_
419 : // @Path: AP_BattMonitor_Synthetic_Current.cpp
420 : // @Group: C_
421 : // @Path: AP_BattMonitor_INA2xx.cpp
422 : // @Group: C_
423 : // @Path: AP_BattMonitor_ESC.cpp
424 : // @Group: C_
425 : // @Path: AP_BattMonitor_INA239.cpp
426 : // @Group: C_
427 : // @Path: AP_BattMonitor_INA3221.cpp
428 : // @Group: C_
429 : // @Path: AP_BattMonitor_AD7091R5.cpp
430 : // @Group: C_
431 : // @Path: AP_BattMonitor_TIBQ76952.cpp
432 : AP_SUBGROUPVARPTR(drivers[11], "C_", 52, AP_BattMonitor, backend_var_info[11]),
433 : #endif
434 :
435 : #if AP_BATT_MONITOR_MAX_INSTANCES > 12
436 : // @Group: D_
437 : // @Path: AP_BattMonitor_Params.cpp
438 : AP_SUBGROUPINFO(_params[12], "D_", 35, AP_BattMonitor, AP_BattMonitor_Params),
439 :
440 : // @Group: D_
441 : // @Path: AP_BattMonitor_Analog.cpp
442 : // @Group: D_
443 : // @Path: AP_BattMonitor_SMBus.cpp
444 : // @Group: D_
445 : // @Path: AP_BattMonitor_Sum.cpp
446 : // @Group: D_
447 : // @Path: AP_BattMonitor_DroneCAN.cpp
448 : // @Group: D_
449 : // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
450 : // @Group: D_
451 : // @Path: AP_BattMonitor_Synthetic_Current.cpp
452 : // @Group: D_
453 : // @Path: AP_BattMonitor_INA2xx.cpp
454 : // @Group: D_
455 : // @Path: AP_BattMonitor_ESC.cpp
456 : // @Group: D_
457 : // @Path: AP_BattMonitor_INA239.cpp
458 : // @Group: D_
459 : // @Path: AP_BattMonitor_INA3221.cpp
460 : // @Group: D_
461 : // @Path: AP_BattMonitor_AD7091R5.cpp
462 : // @Group: D_
463 : // @Path: AP_BattMonitor_TIBQ76952.cpp
464 : AP_SUBGROUPVARPTR(drivers[12], "D_", 53, AP_BattMonitor, backend_var_info[12]),
465 : #endif
466 :
467 : #if AP_BATT_MONITOR_MAX_INSTANCES > 13
468 : // @Group: E_
469 : // @Path: AP_BattMonitor_Params.cpp
470 : AP_SUBGROUPINFO(_params[13], "E_", 36, AP_BattMonitor, AP_BattMonitor_Params),
471 :
472 : // @Group: E_
473 : // @Path: AP_BattMonitor_Analog.cpp
474 : // @Group: E_
475 : // @Path: AP_BattMonitor_SMBus.cpp
476 : // @Group: E_
477 : // @Path: AP_BattMonitor_Sum.cpp
478 : // @Group: E_
479 : // @Path: AP_BattMonitor_DroneCAN.cpp
480 : // @Group: E_
481 : // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
482 : // @Group: E_
483 : // @Path: AP_BattMonitor_Synthetic_Current.cpp
484 : // @Group: E_
485 : // @Path: AP_BattMonitor_INA2xx.cpp
486 : // @Group: E_
487 : // @Path: AP_BattMonitor_ESC.cpp
488 : // @Group: E_
489 : // @Path: AP_BattMonitor_INA239.cpp
490 : // @Group: E_
491 : // @Path: AP_BattMonitor_INA3221.cpp
492 : // @Group: E_
493 : // @Path: AP_BattMonitor_AD7091R5.cpp
494 : // @Group: E_
495 : // @Path: AP_BattMonitor_TIBQ76952.cpp
496 : AP_SUBGROUPVARPTR(drivers[13], "E_", 54, AP_BattMonitor, backend_var_info[13]),
497 : #endif
498 :
499 : #if AP_BATT_MONITOR_MAX_INSTANCES > 14
500 : // @Group: F_
501 : // @Path: AP_BattMonitor_Params.cpp
502 : AP_SUBGROUPINFO(_params[14], "F_", 37, AP_BattMonitor, AP_BattMonitor_Params),
503 :
504 : // @Group: F_
505 : // @Path: AP_BattMonitor_Analog.cpp
506 : // @Group: F_
507 : // @Path: AP_BattMonitor_SMBus.cpp
508 : // @Group: F_
509 : // @Path: AP_BattMonitor_Sum.cpp
510 : // @Group: F_
511 : // @Path: AP_BattMonitor_DroneCAN.cpp
512 : // @Group: F_
513 : // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
514 : // @Group: F_
515 : // @Path: AP_BattMonitor_Synthetic_Current.cpp
516 : // @Group: F_
517 : // @Path: AP_BattMonitor_INA2xx.cpp
518 : // @Group: F_
519 : // @Path: AP_BattMonitor_ESC.cpp
520 : // @Group: F_
521 : // @Path: AP_BattMonitor_INA239.cpp
522 : // @Group: F_
523 : // @Path: AP_BattMonitor_INA3221.cpp
524 : // @Group: F_
525 : // @Path: AP_BattMonitor_AD7091R5.cpp
526 : // @Group: F_
527 : // @Path: AP_BattMonitor_TIBQ76952.cpp
528 : AP_SUBGROUPVARPTR(drivers[14], "F_", 55, AP_BattMonitor, backend_var_info[14]),
529 : #endif
530 :
531 : #if AP_BATT_MONITOR_MAX_INSTANCES > 15
532 : // @Group: G_
533 : // @Path: AP_BattMonitor_Params.cpp
534 : AP_SUBGROUPINFO(_params[15], "G_", 38, AP_BattMonitor, AP_BattMonitor_Params),
535 :
536 : // @Group: G_
537 : // @Path: AP_BattMonitor_Analog.cpp
538 : // @Group: G_
539 : // @Path: AP_BattMonitor_SMBus.cpp
540 : // @Group: G_
541 : // @Path: AP_BattMonitor_Sum.cpp
542 : // @Group: G_
543 : // @Path: AP_BattMonitor_DroneCAN.cpp
544 : // @Group: G_
545 : // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
546 : // @Group: G_
547 : // @Path: AP_BattMonitor_Synthetic_Current.cpp
548 : // @Group: G_
549 : // @Path: AP_BattMonitor_INA2xx.cpp
550 : // @Group: G_
551 : // @Path: AP_BattMonitor_ESC.cpp
552 : // @Group: G_
553 : // @Path: AP_BattMonitor_INA239.cpp
554 : // @Group: G_
555 : // @Path: AP_BattMonitor_INA3221.cpp
556 : // @Group: G_
557 : // @Path: AP_BattMonitor_AD7091R5.cpp
558 : // @Group: G_
559 : // @Path: AP_BattMonitor_TIBQ76952.cpp
560 : AP_SUBGROUPVARPTR(drivers[15], "G_", 56, AP_BattMonitor, backend_var_info[15]),
561 : #endif
562 :
563 : #if AP_BATT_MONITOR_MAX_INSTANCES > 16
564 : #error "AP_BATT_MONITOR_MAX_INSTANCES too large, reset_remaining_mask() will cause an assert above 16"
565 : #endif
566 :
567 : AP_GROUPEND
568 : };
569 :
570 : const AP_Param::GroupInfo *AP_BattMonitor::backend_var_info[AP_BATT_MONITOR_MAX_INSTANCES];
571 :
572 : // Default constructor.
573 : // Note that the Vector/Matrix constructors already implicitly zero
574 : // their values.
575 : //
576 2094 : AP_BattMonitor::AP_BattMonitor(uint32_t log_battery_bit, battery_failsafe_handler_fn_t battery_failsafe_handler_fn, const int8_t *failsafe_priorities) :
577 2094 : _log_battery_bit(log_battery_bit),
578 2094 : _battery_failsafe_handler_fn(battery_failsafe_handler_fn),
579 20940 : _failsafe_priorities(failsafe_priorities)
580 : {
581 2094 : AP_Param::setup_object_defaults(this, var_info);
582 :
583 2094 : if (_singleton != nullptr) {
584 0 : AP_HAL::panic("AP_BattMonitor must be singleton");
585 : }
586 2094 : _singleton = this;
587 2094 : }
588 :
589 : // init - instantiate the battery monitors
590 : void
591 2094 : AP_BattMonitor::init()
592 : {
593 : // check init has not been called before
594 2094 : if (_num_instances != 0) {
595 0 : return;
596 : }
597 :
598 2094 : _highest_failsafe_priority = INT8_MAX;
599 :
600 : #ifdef HAL_BATT_MONITOR_DEFAULT
601 : _params[0]._type.set_default(int8_t(HAL_BATT_MONITOR_DEFAULT));
602 : #endif
603 : #ifdef HAL_BATT2_MONITOR_DEFAULT
604 : _params[1]._type.set_default(int8_t(HAL_BATT2_MONITOR_DEFAULT));
605 : #endif
606 :
607 : // create each instance
608 20940 : for (uint8_t instance=0; instance<AP_BATT_MONITOR_MAX_INSTANCES; instance++) {
609 : // clear out the cell voltages
610 18846 : memset(&state[instance].cell_voltages, 0xFF, sizeof(cells));
611 18846 : state[instance].instance = instance;
612 :
613 18846 : const auto allocation_type = configured_type(instance);
614 18846 : switch (allocation_type) {
615 : #if AP_BATTERY_ANALOG_ENABLED
616 1859 : case Type::ANALOG_VOLTAGE_ONLY:
617 : case Type::ANALOG_VOLTAGE_AND_CURRENT:
618 : case Type::ANALOG_CURRENT_ONLY:
619 1859 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_Analog(*this, state[instance], _params[instance]);
620 1859 : break;
621 : #endif
622 : #if AP_BATTERY_SMBUS_SOLO_ENABLED
623 : case Type::SOLO:
624 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_SMBus_Solo(*this, state[instance], _params[instance]);
625 : break;
626 : #endif
627 : #if AP_BATTERY_SMBUS_GENERIC_ENABLED
628 0 : case Type::SMBus_Generic:
629 0 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_SMBus_Generic(*this, state[instance], _params[instance]);
630 0 : break;
631 : #endif
632 : #if AP_BATTERY_SMBUS_SUI_ENABLED
633 0 : case Type::SUI3:
634 0 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_SMBus_SUI(*this, state[instance], _params[instance], 3);
635 0 : break;
636 0 : case Type::SUI6:
637 0 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_SMBus_SUI(*this, state[instance], _params[instance], 6);
638 0 : break;
639 : #endif
640 : #if AP_BATTERY_SMBUS_MAXELL_ENABLED
641 2 : case Type::MAXELL:
642 2 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_SMBus_Maxell(*this, state[instance], _params[instance]);
643 2 : break;
644 : #endif
645 : #if AP_BATTERY_SMBUS_ROTOYE_ENABLED
646 0 : case Type::Rotoye:
647 0 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_SMBus_Rotoye(*this, state[instance], _params[instance]);
648 0 : break;
649 : #endif
650 : #if AP_BATTERY_SMBUS_NEODESIGN_ENABLED
651 0 : case Type::NeoDesign:
652 0 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_SMBus_NeoDesign(*this, state[instance], _params[instance]);
653 0 : break;
654 : #endif
655 : #if AP_BATTERY_BEBOP_ENABLED
656 : case Type::BEBOP:
657 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_Bebop(*this, state[instance], _params[instance]);
658 : break;
659 : #endif
660 : #if AP_BATTERY_UAVCAN_BATTERYINFO_ENABLED
661 0 : case Type::UAVCAN_BatteryInfo:
662 0 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_DroneCAN(*this, state[instance], AP_BattMonitor_DroneCAN::UAVCAN_BATTERY_INFO, _params[instance]);
663 0 : break;
664 : #endif
665 : #if AP_BATTERY_ESC_ENABLED
666 0 : case Type::BLHeliESC:
667 0 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_ESC(*this, state[instance], _params[instance]);
668 0 : break;
669 : #endif
670 : #if AP_BATTERY_SUM_ENABLED
671 0 : case Type::Sum:
672 0 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_Sum(*this, state[instance], _params[instance], instance);
673 0 : break;
674 : #endif
675 : #if AP_BATTERY_FUELFLOW_ENABLED
676 0 : case Type::FuelFlow:
677 0 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_FuelFlow(*this, state[instance], _params[instance]);
678 0 : break;
679 : #endif // AP_BATTERY_FUELFLOW_ENABLED
680 : #if AP_BATTERY_FUELLEVEL_PWM_ENABLED
681 0 : case Type::FuelLevel_PWM:
682 0 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_FuelLevel_PWM(*this, state[instance], _params[instance]);
683 0 : break;
684 : #endif // AP_BATTERY_FUELLEVEL_PWM_ENABLED
685 : #if AP_BATTERY_FUELLEVEL_ANALOG_ENABLED
686 0 : case Type::FuelLevel_Analog:
687 0 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_FuelLevel_Analog(*this, state[instance], _params[instance]);
688 0 : break;
689 : #endif // AP_BATTERY_FUELLEVEL_ANALOG_ENABLED
690 : #if HAL_GENERATOR_ENABLED
691 16 : case Type::GENERATOR_ELEC:
692 16 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_Generator_Elec(*this, state[instance], _params[instance]);
693 16 : break;
694 14 : case Type::GENERATOR_FUEL:
695 14 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_Generator_FuelLevel(*this, state[instance], _params[instance]);
696 14 : break;
697 : #endif // HAL_GENERATOR_ENABLED
698 : #if AP_BATTERY_INA2XX_ENABLED
699 0 : case Type::INA2XX:
700 0 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_INA2XX(*this, state[instance], _params[instance]);
701 0 : break;
702 : #endif
703 : #if AP_BATTERY_LTC2946_ENABLED
704 : case Type::LTC2946:
705 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_LTC2946(*this, state[instance], _params[instance]);
706 : break;
707 : #endif
708 : #if HAL_TORQEEDO_ENABLED
709 0 : case Type::Torqeedo:
710 0 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_Torqeedo(*this, state[instance], _params[instance]);
711 0 : break;
712 : #endif
713 : #if AP_BATTERY_SYNTHETIC_CURRENT_ENABLED
714 0 : case Type::Analog_Volt_Synthetic_Current:
715 0 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_Synthetic_Current(*this, state[instance], _params[instance]);
716 0 : break;
717 : #endif
718 : #if AP_BATTERY_INA239_ENABLED
719 : case Type::INA239_SPI:
720 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_INA239(*this, state[instance], _params[instance]);
721 : break;
722 : #endif
723 : #if AP_BATTERY_EFI_ENABLED
724 0 : case Type::EFI:
725 0 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_EFI(*this, state[instance], _params[instance]);
726 0 : break;
727 : #endif // AP_BATTERY_EFI_ENABLED
728 : #if AP_BATTERY_AD7091R5_ENABLED
729 0 : case Type::AD7091R5:
730 0 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_AD7091R5(*this, state[instance], _params[instance]);
731 0 : break;
732 : #endif// AP_BATTERY_AD7091R5_ENABLED
733 : #if AP_BATTERY_SCRIPTING_ENABLED
734 0 : case Type::Scripting:
735 0 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_Scripting(*this, state[instance], _params[instance]);
736 0 : break;
737 : #endif // AP_BATTERY_SCRIPTING_ENABLED
738 : #if AP_BATTERY_INA3221_ENABLED
739 6 : case Type::INA3221:
740 6 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_INA3221(*this, state[instance], _params[instance]);
741 6 : break;
742 : #endif // AP_BATTERY_INA3221_ENABLED
743 : #if AP_BATTERY_TIBQ76952_ENABLED
744 : case Type::TIBQ76952_I2C:
745 : drivers[instance] = NEW_NOTHROW AP_BattMonitor_TIBQ76952(*this, state[instance], _params[instance]);
746 : break;
747 : #endif // AP_BATTERY_TIBQ76952_ENABLED
748 16949 : case Type::NONE:
749 : default:
750 16949 : break;
751 : }
752 18846 : if (drivers[instance] != nullptr) {
753 1897 : state[instance].type = allocation_type;
754 : }
755 : // if the backend has some local parameters then make those available in the tree
756 18846 : if (drivers[instance] && state[instance].var_info) {
757 1867 : backend_var_info[instance] = state[instance].var_info;
758 1867 : AP_Param::load_object_from_eeprom(drivers[instance], backend_var_info[instance]);
759 :
760 : // param count could have changed
761 1867 : AP_Param::invalidate_count();
762 : }
763 :
764 : // call init function for each backend
765 18846 : if (drivers[instance] != nullptr) {
766 1897 : drivers[instance]->init();
767 : // _num_instances is actually the index for looping over instances
768 : // the user may have BATT_MONITOR=0 and BATT2_MONITOR=7, in which case
769 : // there will be a gap, but as we always check for drivers[instances] being nullptr
770 : // this is safe
771 1897 : _num_instances = instance + 1;
772 : }
773 : }
774 : }
775 :
776 : // read - For all active instances read voltage & current; log BAT, BCL, POWR, MCU
777 1506380 : void AP_BattMonitor::read()
778 : {
779 : #if HAL_LOGGING_ENABLED
780 1506380 : AP_Logger *logger = AP_Logger::get_singleton();
781 1506380 : if (logger != nullptr && logger->should_log(_log_battery_bit)) {
782 1444293 : logger->Write_Power();
783 : }
784 : #endif
785 :
786 1506380 : const uint32_t now_ms = AP_HAL::millis();
787 2766930 : for (uint8_t i=0; i<_num_instances; i++) {
788 1260550 : if (drivers[i] == nullptr) {
789 66 : continue;
790 : }
791 1260484 : if (allocated_type(i) != configured_type(i)) {
792 294 : continue;
793 : }
794 : // allow run-time disabling; this is technically redundant
795 1260190 : if (configured_type(i) == Type::NONE) {
796 0 : continue;
797 : }
798 1260190 : drivers[i]->read();
799 1260190 : drivers[i]->update_resistance_estimate();
800 :
801 : #if AP_BATTERY_ESC_TELEM_OUTBOUND_ENABLED
802 : drivers[i]->update_esc_telem_outbound();
803 : #endif
804 :
805 : // Update last heathy timestamp
806 1260190 : if (state[i].healthy) {
807 1259414 : state[i].last_healthy_ms = now_ms;
808 : }
809 :
810 : #if HAL_LOGGING_ENABLED
811 1260190 : if (logger != nullptr && logger->should_log(_log_battery_bit)) {
812 1214233 : const uint64_t time_us = AP_HAL::micros64();
813 1214233 : drivers[i]->Log_Write_BAT(i, time_us);
814 1214233 : drivers[i]->Log_Write_BCL(i, time_us);
815 : }
816 : #endif
817 : }
818 :
819 1506380 : check_failsafes();
820 :
821 1506380 : checkPoweringOff();
822 1506380 : }
823 :
824 : // healthy - returns true if monitor is functioning
825 2625976 : bool AP_BattMonitor::healthy(uint8_t instance) const {
826 2625976 : return instance < _num_instances && state[instance].healthy;
827 : }
828 :
829 : /// voltage - returns battery voltage in volts
830 1482145 : float AP_BattMonitor::voltage(uint8_t instance) const
831 : {
832 1482145 : if (instance < _num_instances) {
833 1446104 : return state[instance].voltage;
834 : } else {
835 36041 : return 0.0f;
836 : }
837 : }
838 :
839 : /// get voltage with sag removed (based on battery current draw and resistance)
840 : /// this will always be greater than or equal to the raw voltage
841 36254743 : float AP_BattMonitor::voltage_resting_estimate(uint8_t instance) const
842 : {
843 36254743 : if (instance < _num_instances && drivers[instance] != nullptr) {
844 28831805 : return drivers[instance]->voltage_resting_estimate();
845 : } else {
846 7422938 : return 0.0f;
847 : }
848 : }
849 :
850 : /// voltage - returns battery voltage in volts for GCS, may be resting voltage if option enabled
851 1985161 : float AP_BattMonitor::gcs_voltage(uint8_t instance) const
852 : {
853 1985161 : if (instance >= _num_instances || drivers[instance] == nullptr) {
854 249516 : return 0.0f;
855 : }
856 1735645 : if (drivers[instance]->option_is_set(AP_BattMonitor_Params::Options::GCS_Resting_Voltage)) {
857 1078 : return voltage_resting_estimate(instance);
858 : }
859 1734567 : return state[instance].voltage;
860 : }
861 :
862 868230 : bool AP_BattMonitor::option_is_set(uint8_t instance, AP_BattMonitor_Params::Options option) const
863 : {
864 868230 : if (instance >= _num_instances || drivers[instance] == nullptr) {
865 0 : return false;
866 : }
867 868230 : return drivers[instance]->option_is_set(option);
868 : }
869 :
870 : /// current_amps - returns the instantaneous current draw in amperes
871 2083859 : bool AP_BattMonitor::current_amps(float ¤t, uint8_t instance) const {
872 2083859 : if ((instance < _num_instances) && (drivers[instance] != nullptr) && drivers[instance]->has_current()) {
873 1834344 : current = state[instance].current_amps;
874 1834344 : return true;
875 : } else {
876 249515 : return false;
877 : }
878 : }
879 :
880 : /// consumed_mah - returns total current drawn since start-up in milliampere.hours
881 868907 : bool AP_BattMonitor::consumed_mah(float &mah, const uint8_t instance) const {
882 868907 : if ((instance < _num_instances) && (drivers[instance] != nullptr) && drivers[instance]->has_current()) {
883 868907 : const float consumed_mah = state[instance].consumed_mah;
884 868907 : if (isnan(consumed_mah)) {
885 0 : return false;
886 : }
887 868907 : mah = consumed_mah;
888 868907 : return true;
889 : } else {
890 0 : return false;
891 : }
892 : }
893 :
894 : /// consumed_wh - returns energy consumed since start-up in Watt.hours
895 868214 : bool AP_BattMonitor::consumed_wh(float &wh, const uint8_t instance) const {
896 868214 : if (instance < _num_instances && drivers[instance] != nullptr && drivers[instance]->has_consumed_energy()) {
897 867639 : wh = state[instance].consumed_wh;
898 867639 : return true;
899 : } else {
900 575 : return false;
901 : }
902 : }
903 :
904 : /// capacity_remaining_pct - returns true if the percentage is valid and writes to percentage argument
905 1987288 : bool AP_BattMonitor::capacity_remaining_pct(uint8_t &percentage, uint8_t instance) const
906 : {
907 1987288 : if (instance < _num_instances && drivers[instance] != nullptr) {
908 1737772 : return drivers[instance]->capacity_remaining_pct(percentage);
909 : }
910 249516 : return false;
911 : }
912 :
913 : /// time_remaining - returns remaining battery time
914 868214 : bool AP_BattMonitor::time_remaining(uint32_t &seconds, uint8_t instance) const
915 : {
916 868214 : if (instance < _num_instances && drivers[instance] != nullptr && state[instance].has_time_remaining) {
917 0 : seconds = state[instance].time_remaining;
918 0 : return true;
919 : }
920 868214 : return false;
921 : }
922 :
923 : /// pack_capacity_mah - returns the capacity of the battery pack in mAh when the pack is full
924 131 : int32_t AP_BattMonitor::pack_capacity_mah(uint8_t instance) const
925 : {
926 131 : if (instance < AP_BATT_MONITOR_MAX_INSTANCES) {
927 131 : return _params[instance]._pack_capacity;
928 : } else {
929 0 : return 0;
930 : }
931 : }
932 :
933 1506380 : void AP_BattMonitor::check_failsafes(void)
934 : {
935 1506380 : if (hal.util->get_soft_armed()) {
936 1833905 : for (uint8_t i = 0; i < _num_instances; i++) {
937 826805 : if (drivers[i] == nullptr) {
938 46 : continue;
939 : }
940 :
941 826759 : const Failsafe type = drivers[i]->update_failsafes();
942 826759 : if (type <= state[i].failsafe) {
943 826737 : continue;
944 : }
945 :
946 22 : int8_t action = 0;
947 22 : const char *type_str = nullptr;
948 22 : switch (type) {
949 0 : case Failsafe::None:
950 0 : continue; // should not have been called in this case
951 1 : case Failsafe::Unhealthy:
952 : // Report only for unhealthy, could add action param in the future
953 1 : action = 0;
954 1 : type_str = "missing, last:";
955 1 : break;
956 14 : case Failsafe::Low:
957 14 : action = _params[i]._failsafe_low_action;
958 14 : type_str = "low";
959 14 : break;
960 7 : case Failsafe::Critical:
961 7 : action = _params[i]._failsafe_critical_action;
962 7 : type_str = "critical";
963 7 : break;
964 : }
965 :
966 22 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Battery %d is %s %.2fV used %.0f mAh", i + 1, type_str,
967 : (double)voltage(i), (double)state[i].consumed_mah);
968 22 : _has_triggered_failsafe = true;
969 : #ifndef HAL_BUILD_AP_PERIPH
970 22 : AP_Notify::flags.failsafe_battery = true;
971 : #endif
972 22 : state[i].failsafe = type;
973 :
974 : // map the desired failsafe action to a priority level
975 22 : int8_t priority = 0;
976 22 : if (_failsafe_priorities != nullptr) {
977 95 : while (_failsafe_priorities[priority] != -1) {
978 91 : if (_failsafe_priorities[priority] == action) {
979 18 : break;
980 : }
981 73 : priority++;
982 : }
983 :
984 : }
985 :
986 : // trigger failsafe if the action was equal or higher priority
987 : // It's valid to retrigger the same action if a different battery provoked the event
988 22 : if (priority <= _highest_failsafe_priority) {
989 22 : _battery_failsafe_handler_fn(type_str, action);
990 22 : _highest_failsafe_priority = priority;
991 : }
992 : }
993 : }
994 1506380 : }
995 :
996 : // return true if any battery is pushing too much power
997 7338760 : bool AP_BattMonitor::overpower_detected() const
998 : {
999 : #if AP_BATTERY_WATT_MAX_ENABLED && APM_BUILD_TYPE(APM_BUILD_ArduPlane)
1000 8760649 : for (uint8_t instance = 0; instance < _num_instances; instance++) {
1001 1421889 : if (overpower_detected(instance)) {
1002 0 : return true;
1003 : }
1004 : }
1005 : #endif
1006 7338760 : return false;
1007 : }
1008 :
1009 1421889 : bool AP_BattMonitor::overpower_detected(uint8_t instance) const
1010 : {
1011 : #if AP_BATTERY_WATT_MAX_ENABLED && APM_BUILD_TYPE(APM_BUILD_ArduPlane)
1012 1421889 : if (instance < _num_instances && _params[instance]._watt_max > 0) {
1013 0 : const float power = state[instance].current_amps * state[instance].voltage;
1014 0 : return state[instance].healthy && (power > _params[instance]._watt_max);
1015 : }
1016 : #endif
1017 1421889 : return false;
1018 : }
1019 :
1020 868214 : bool AP_BattMonitor::has_cell_voltages(const uint8_t instance) const
1021 : {
1022 868214 : if (instance < _num_instances && drivers[instance] != nullptr) {
1023 868214 : return drivers[instance]->has_cell_voltages();
1024 : }
1025 :
1026 0 : return false;
1027 : }
1028 :
1029 : // return the current cell voltages, returns the first monitor instances cells if the instance is out of range
1030 302 : const AP_BattMonitor::cells & AP_BattMonitor::get_cell_voltages(const uint8_t instance) const
1031 : {
1032 302 : if (instance >= AP_BATT_MONITOR_MAX_INSTANCES) {
1033 0 : return state[AP_BATT_PRIMARY_INSTANCE].cell_voltages;
1034 : } else {
1035 302 : return state[instance].cell_voltages;
1036 : }
1037 : }
1038 :
1039 : // get once cell voltage (for scripting)
1040 0 : bool AP_BattMonitor::get_cell_voltage(uint8_t instance, uint8_t cell, float &voltage) const
1041 : {
1042 0 : if (!has_cell_voltages(instance) ||
1043 : cell >= AP_BATT_MONITOR_CELLS_MAX) {
1044 0 : return false;
1045 : }
1046 0 : const auto &cell_voltages = get_cell_voltages(instance);
1047 0 : const uint16_t voltage_mv = cell_voltages.cells[cell];
1048 0 : if (voltage_mv == 0 || voltage_mv == UINT16_MAX) {
1049 : // UINT16_MAX is used as invalid indicator
1050 0 : return false;
1051 : }
1052 0 : voltage = voltage_mv*0.001;
1053 0 : return true;
1054 : }
1055 :
1056 : // returns true if there is a temperature reading
1057 868214 : bool AP_BattMonitor::get_temperature(float &temperature, const uint8_t instance) const
1058 : {
1059 868214 : if (instance >= _num_instances || drivers[instance] == nullptr) {
1060 0 : return false;
1061 : }
1062 :
1063 868214 : return drivers[instance]->get_temperature(temperature);
1064 : }
1065 :
1066 : #if AP_TEMPERATURE_SENSOR_ENABLED
1067 : // return true when successfully setting a battery temperature from an external source by instance
1068 0 : bool AP_BattMonitor::set_temperature(const float temperature, const uint8_t instance)
1069 : {
1070 0 : if (instance >= _num_instances || drivers[instance] == nullptr) {
1071 0 : return false;
1072 : }
1073 0 : state[instance].temperature_external = temperature;
1074 0 : state[instance].temperature_external_use = true;
1075 0 : return true;
1076 : }
1077 :
1078 : // return true when successfully setting a battery temperature from an external source by serial_number
1079 0 : bool AP_BattMonitor::set_temperature_by_serial_number(const float temperature, const int32_t serial_number)
1080 : {
1081 0 : bool success = false;
1082 0 : for (uint8_t i = 0; i < _num_instances; i++) {
1083 0 : if (drivers[i] != nullptr && get_serial_number(i) == serial_number) {
1084 0 : success |= set_temperature(temperature, i);
1085 : }
1086 : }
1087 0 : return success;
1088 : }
1089 : #endif // AP_TEMPERATURE_SENSOR_ENABLED
1090 :
1091 : // return true if cycle count can be provided and fills in cycles argument
1092 0 : bool AP_BattMonitor::get_cycle_count(uint8_t instance, uint16_t &cycles) const
1093 : {
1094 0 : if (instance >= _num_instances || (drivers[instance] == nullptr)) {
1095 0 : return false;
1096 : }
1097 0 : return drivers[instance]->get_cycle_count(cycles);
1098 : }
1099 :
1100 137768 : bool AP_BattMonitor::arming_checks(size_t buflen, char *buffer) const
1101 : {
1102 137768 : char temp_buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1] {};
1103 :
1104 1374147 : for (uint8_t i = 0; i < AP_BATT_MONITOR_MAX_INSTANCES; i++) {
1105 1236844 : const auto expected_type = configured_type(i);
1106 :
1107 1236844 : if (drivers[i] == nullptr && expected_type == Type::NONE) {
1108 1109239 : continue;
1109 : }
1110 :
1111 : #if !AP_BATTERY_SUM_ENABLED
1112 : // CONVERSION - Added Sep 2024 for ArduPilot 4.6 as we are
1113 : // removing the SUM backend on 1MB boards. Give a
1114 : // more-specific error for the sum backend:
1115 : if (expected_type == Type::Sum) {
1116 : hal.util->snprintf(buffer, buflen, "Battery %d %s", i + 1, "feature BATTERY_SUM not available");
1117 : return false;
1118 : }
1119 : #endif
1120 :
1121 127605 : if (drivers[i] == nullptr || allocated_type(i) != expected_type) {
1122 118 : hal.util->snprintf(buffer, buflen, "Battery %d %s", i + 1, "unhealthy");
1123 118 : return false;
1124 : }
1125 :
1126 127487 : if (!drivers[i]->arming_checks(temp_buffer, sizeof(temp_buffer))) {
1127 347 : hal.util->snprintf(buffer, buflen, "Battery %d %s", i + 1, temp_buffer);
1128 347 : return false;
1129 : }
1130 : }
1131 :
1132 137303 : return true;
1133 : }
1134 :
1135 : // Check's each smart battery instance for its powering off state and broadcasts notifications
1136 1506380 : void AP_BattMonitor::checkPoweringOff(void)
1137 : {
1138 2766930 : for (uint8_t i = 0; i < _num_instances; i++) {
1139 1260550 : if (state[i].is_powering_off && !state[i].powerOffNotified) {
1140 : #ifndef HAL_BUILD_AP_PERIPH
1141 : // Set the AP_Notify flag, which plays the power off tones
1142 0 : AP_Notify::flags.powering_off = true;
1143 : #endif
1144 :
1145 : // Send a Mavlink broadcast announcing the shutdown
1146 : #if HAL_GCS_ENABLED
1147 0 : mavlink_command_long_t cmd_msg{};
1148 0 : cmd_msg.command = MAV_CMD_POWER_OFF_INITIATED;
1149 0 : cmd_msg.param1 = i+1;
1150 0 : GCS_MAVLINK::send_to_components(MAVLINK_MSG_ID_COMMAND_LONG, (char*)&cmd_msg, sizeof(cmd_msg));
1151 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Vehicle %d battery %d is powering off", mavlink_system.sysid, i+1);
1152 : #endif
1153 :
1154 : // only send this once
1155 0 : state[i].powerOffNotified = true;
1156 : }
1157 : }
1158 1506380 : }
1159 :
1160 : /*
1161 : reset battery remaining percentage for batteries that integrate to
1162 : calculate percentage remaining
1163 : */
1164 18 : bool AP_BattMonitor::reset_remaining_mask(uint16_t battery_mask, float percentage)
1165 : {
1166 : static_assert(AP_BATT_MONITOR_MAX_INSTANCES <= 16, "More batteries are enabled then can be reset");
1167 18 : bool ret = true;
1168 18 : Failsafe highest_failsafe = Failsafe::None;
1169 56 : for (uint8_t i = 0; i < _num_instances; i++) {
1170 38 : if ((1U<<i) & battery_mask) {
1171 18 : if (drivers[i] != nullptr) {
1172 18 : ret &= drivers[i]->reset_remaining(percentage);
1173 : } else {
1174 0 : ret = false;
1175 : }
1176 : }
1177 38 : if (state[i].failsafe > highest_failsafe) {
1178 0 : highest_failsafe = state[i].failsafe;
1179 : }
1180 : }
1181 :
1182 : // If all backends are not in failsafe then set overall failsafe state
1183 18 : if (highest_failsafe == Failsafe::None) {
1184 18 : _highest_failsafe_priority = INT8_MAX;
1185 18 : _has_triggered_failsafe = false;
1186 : // and reset notify flag
1187 18 : AP_Notify::flags.failsafe_battery = false;
1188 : }
1189 18 : return ret;
1190 : }
1191 :
1192 : // Returns the mavlink charge state. The following mavlink charge states are not used
1193 : // MAV_BATTERY_CHARGE_STATE_EMERGENCY , MAV_BATTERY_CHARGE_STATE_FAILED
1194 : // MAV_BATTERY_CHARGE_STATE_UNHEALTHY, MAV_BATTERY_CHARGE_STATE_CHARGING
1195 868214 : MAV_BATTERY_CHARGE_STATE AP_BattMonitor::get_mavlink_charge_state(const uint8_t instance) const
1196 : {
1197 868214 : if (instance >= _num_instances) {
1198 0 : return MAV_BATTERY_CHARGE_STATE_UNDEFINED;
1199 : }
1200 :
1201 868214 : switch (state[instance].failsafe) {
1202 :
1203 864627 : case Failsafe::None:
1204 : case Failsafe::Unhealthy:
1205 864627 : if (get_mavlink_fault_bitmask(instance) != 0 || !healthy(instance)) {
1206 1327 : return MAV_BATTERY_CHARGE_STATE_UNHEALTHY;
1207 : }
1208 863300 : return MAV_BATTERY_CHARGE_STATE_OK;
1209 :
1210 2038 : case Failsafe::Low:
1211 2038 : return MAV_BATTERY_CHARGE_STATE_LOW;
1212 :
1213 1549 : case Failsafe::Critical:
1214 1549 : return MAV_BATTERY_CHARGE_STATE_CRITICAL;
1215 : }
1216 :
1217 : // Should not reach this
1218 0 : return MAV_BATTERY_CHARGE_STATE_UNDEFINED;
1219 : }
1220 :
1221 : // Returns mavlink fault state
1222 1732841 : uint32_t AP_BattMonitor::get_mavlink_fault_bitmask(const uint8_t instance) const
1223 : {
1224 1732841 : if (instance >= _num_instances || drivers[instance] == nullptr) {
1225 0 : return 0;
1226 : }
1227 1732841 : return drivers[instance]->get_mavlink_fault_bitmask();
1228 : }
1229 :
1230 : // return true if state of health (as a percentage) can be provided and fills in soh_pct argument
1231 0 : bool AP_BattMonitor::get_state_of_health_pct(uint8_t instance, uint8_t &soh_pct) const
1232 : {
1233 0 : if (instance >= _num_instances || drivers[instance] == nullptr) {
1234 0 : return false;
1235 : }
1236 0 : return drivers[instance]->get_state_of_health_pct(soh_pct);
1237 : }
1238 :
1239 : // Enable/Disable (Turn on/off) power to all backends who are MPPTs or BMSs
1240 0 : void AP_BattMonitor::set_powered_state_to_all(const bool power_on)
1241 : {
1242 0 : for (uint8_t i=0; i < _num_instances; i++) {
1243 0 : set_powered_state(i, power_on);
1244 : }
1245 0 : }
1246 :
1247 : // Enable/Disable (Turn on/off) power. When disabled, the MPPT or BMS does not
1248 : // supply energy to the system regardless if it's capable to or not. When enabled
1249 : // it will supply energy if available.
1250 0 : void AP_BattMonitor::set_powered_state(const uint8_t instance, const bool power_on)
1251 : {
1252 0 : if (instance < _num_instances && drivers[instance] != nullptr) {
1253 0 : drivers[instance]->set_powered_state(power_on);
1254 : }
1255 0 : }
1256 :
1257 : /*
1258 : check that all configured battery monitors are healthy
1259 : */
1260 2238067 : bool AP_BattMonitor::healthy() const
1261 : {
1262 3997038 : for (uint8_t i=0; i< _num_instances; i++) {
1263 1761625 : if (allocated_type(i) != configured_type(i)) {
1264 250 : return false;
1265 : }
1266 : // allow run-time disabling; this is technically redundant
1267 1761375 : if (configured_type(i) == Type::NONE) {
1268 26 : continue;
1269 : }
1270 1761349 : if (!healthy(i)) {
1271 2404 : return false;
1272 : }
1273 : }
1274 2235413 : return true;
1275 : }
1276 :
1277 : #if AP_BATTERY_SCRIPTING_ENABLED
1278 : /*
1279 : handle state update from a lua script
1280 : */
1281 0 : bool AP_BattMonitor::handle_scripting(uint8_t idx, const BattMonitorScript_State &_state)
1282 : {
1283 0 : if (idx >= _num_instances) {
1284 0 : return false;
1285 : }
1286 0 : if (drivers[idx] == nullptr) {
1287 0 : return false;
1288 : }
1289 0 : return drivers[idx]->handle_scripting(_state);
1290 : }
1291 : #endif
1292 :
1293 : // set battery BMS sleep timeout in seconds
1294 : // set to zero to disable sleep
1295 0 : void AP_BattMonitor::set_sleep_timeout(uint8_t instance, uint16_t timeout_sec)
1296 : {
1297 0 : if (instance >= _num_instances || drivers[instance] == nullptr) {
1298 0 : return;
1299 : }
1300 0 : drivers[instance]->set_sleep_timeout(timeout_sec);
1301 : }
1302 :
1303 : namespace AP {
1304 :
1305 65052479 : AP_BattMonitor &battery()
1306 : {
1307 65052479 : return *AP_BattMonitor::get_singleton();
1308 : }
1309 :
1310 : };
1311 :
1312 : #endif // AP_BATTERY_ENABLED
|
