CoCalc -- AP_BattMonitor.cpp

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/libraries/AP_BattMonitor/AP_BattMonitor.cpp
Views: ¹⁷⁹⁸
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

32
#include <AP_HAL/AP_HAL.h>
33

34
#if HAL_ENABLE_DRONECAN_DRIVERS
35
#include "AP_BattMonitor_DroneCAN.h"
36
#endif
37

38
#include <AP_Vehicle/AP_Vehicle_Type.h>
39
#include <AP_Logger/AP_Logger.h>
40
#include <GCS_MAVLink/GCS.h>
41
#include <AP_Notify/AP_Notify.h>
42

43
extern const AP_HAL::HAL& hal;
44

45
AP_BattMonitor *AP_BattMonitor::_singleton;
46

47
const AP_Param::GroupInfo AP_BattMonitor::var_info[] = {
48
    // 0 - 18, 20- 22 used by old parameter indexes
49

50
    // Monitor 1
51

52
    // @Group: _
53
    // @Path: AP_BattMonitor_Params.cpp
54
    AP_SUBGROUPINFO(_params[0], "_", 23, AP_BattMonitor, AP_BattMonitor_Params),
55

56
    // @Group: _
57
    // @Path: AP_BattMonitor_Analog.cpp
58
    // @Group: _
59
    // @Path: AP_BattMonitor_SMBus.cpp
60
    // @Group: _
61
    // @Path: AP_BattMonitor_Sum.cpp
62
    // @Group: _
63
    // @Path: AP_BattMonitor_DroneCAN.cpp
64
    // @Group: _
65
    // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
66
    // @Group: _
67
    // @Path: AP_BattMonitor_Synthetic_Current.cpp
68
    // @Group: _
69
    // @Path: AP_BattMonitor_INA2xx.cpp
70
    // @Group: _
71
    // @Path: AP_BattMonitor_ESC.cpp
72
    // @Group: _
73
    // @Path: AP_BattMonitor_INA239.cpp
74
    // @Group: _
75
    // @Path: AP_BattMonitor_INA3221.cpp
76
    // @Group: _
77
    // @Path: AP_BattMonitor_AD7091R5.cpp
78
    AP_SUBGROUPVARPTR(drivers[0], "_", 41, AP_BattMonitor, backend_var_info[0]),
79

80
#if AP_BATT_MONITOR_MAX_INSTANCES > 1
81
    // @Group: 2_
82
    // @Path: AP_BattMonitor_Params.cpp
83
    AP_SUBGROUPINFO(_params[1], "2_", 24, AP_BattMonitor, AP_BattMonitor_Params),
84

85
    // @Group: 2_
86
    // @Path: AP_BattMonitor_Analog.cpp
87
    // @Group: 2_
88
    // @Path: AP_BattMonitor_SMBus.cpp
89
    // @Group: 2_
90
    // @Path: AP_BattMonitor_Sum.cpp
91
    // @Group: 2_
92
    // @Path: AP_BattMonitor_DroneCAN.cpp
93
    // @Group: 2_
94
    // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
95
    // @Group: 2_
96
    // @Path: AP_BattMonitor_Synthetic_Current.cpp
97
    // @Group: 2_
98
    // @Path: AP_BattMonitor_INA2xx.cpp
99
    // @Group: 2_
100
    // @Path: AP_BattMonitor_ESC.cpp
101
    // @Group: 2_
102
    // @Path: AP_BattMonitor_INA239.cpp
103
    // @Group: 2_
104
    // @Path: AP_BattMonitor_INA3221.cpp
105
    // @Group: 2_
106
    // @Path: AP_BattMonitor_AD7091R5.cpp
107
    AP_SUBGROUPVARPTR(drivers[1], "2_", 42, AP_BattMonitor, backend_var_info[1]),
108
#endif
109

110
#if AP_BATT_MONITOR_MAX_INSTANCES > 2
111
    // @Group: 3_
112
    // @Path: AP_BattMonitor_Params.cpp
113
    AP_SUBGROUPINFO(_params[2], "3_", 25, AP_BattMonitor, AP_BattMonitor_Params),
114

115
    // @Group: 3_
116
    // @Path: AP_BattMonitor_Analog.cpp
117
    // @Group: 3_
118
    // @Path: AP_BattMonitor_SMBus.cpp
119
    // @Group: 3_
120
    // @Path: AP_BattMonitor_Sum.cpp
121
    // @Group: 3_
122
    // @Path: AP_BattMonitor_DroneCAN.cpp
123
    // @Group: 3_
124
    // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
125
    // @Group: 3_
126
    // @Path: AP_BattMonitor_Synthetic_Current.cpp
127
    // @Group: 3_
128
    // @Path: AP_BattMonitor_INA2xx.cpp
129
    // @Group: 3_
130
    // @Path: AP_BattMonitor_ESC.cpp
131
    // @Group: 3_
132
    // @Path: AP_BattMonitor_INA239.cpp
133
    // @Group: 3_
134
    // @Path: AP_BattMonitor_INA3221.cpp
135
    // @Group: 3_
136
    // @Path: AP_BattMonitor_AD7091R5.cpp
137
    AP_SUBGROUPVARPTR(drivers[2], "3_", 43, AP_BattMonitor, backend_var_info[2]),
138
#endif
139

140
#if AP_BATT_MONITOR_MAX_INSTANCES > 3
141
    // @Group: 4_
142
    // @Path: AP_BattMonitor_Params.cpp
143
    AP_SUBGROUPINFO(_params[3], "4_", 26, AP_BattMonitor, AP_BattMonitor_Params),
144

145
    // @Group: 4_
146
    // @Path: AP_BattMonitor_Analog.cpp
147
    // @Group: 4_
148
    // @Path: AP_BattMonitor_SMBus.cpp
149
    // @Group: 4_
150
    // @Path: AP_BattMonitor_Sum.cpp
151
    // @Group: 4_
152
    // @Path: AP_BattMonitor_DroneCAN.cpp
153
    // @Group: 4_
154
    // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
155
    // @Group: 4_
156
    // @Path: AP_BattMonitor_Synthetic_Current.cpp
157
    // @Group: 4_
158
    // @Path: AP_BattMonitor_INA2xx.cpp
159
    // @Group: 4_
160
    // @Path: AP_BattMonitor_ESC.cpp
161
    // @Group: 4_
162
    // @Path: AP_BattMonitor_INA239.cpp
163
    // @Group: 4_
164
    // @Path: AP_BattMonitor_INA3221.cpp
165
    // @Group: 4_
166
    // @Path: AP_BattMonitor_AD7091R5.cpp
167
    AP_SUBGROUPVARPTR(drivers[3], "4_", 44, AP_BattMonitor, backend_var_info[3]),
168
#endif
169

170
#if AP_BATT_MONITOR_MAX_INSTANCES > 4
171
    // @Group: 5_
172
    // @Path: AP_BattMonitor_Params.cpp
173
    AP_SUBGROUPINFO(_params[4], "5_", 27, AP_BattMonitor, AP_BattMonitor_Params),
174

175
    // @Group: 5_
176
    // @Path: AP_BattMonitor_Analog.cpp
177
    // @Group: 5_
178
    // @Path: AP_BattMonitor_SMBus.cpp
179
    // @Group: 5_
180
    // @Path: AP_BattMonitor_Sum.cpp
181
    // @Group: 5_
182
    // @Path: AP_BattMonitor_DroneCAN.cpp
183
    // @Group: 5_
184
    // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
185
    // @Group: 5_
186
    // @Path: AP_BattMonitor_Synthetic_Current.cpp
187
    // @Group: 5_
188
    // @Path: AP_BattMonitor_INA2xx.cpp
189
    // @Group: 5_
190
    // @Path: AP_BattMonitor_ESC.cpp
191
    // @Group: 5_
192
    // @Path: AP_BattMonitor_INA239.cpp
193
    // @Group: 5_
194
    // @Path: AP_BattMonitor_INA3221.cpp
195
    // @Group: 5_
196
    // @Path: AP_BattMonitor_AD7091R5.cpp
197
    AP_SUBGROUPVARPTR(drivers[4], "5_", 45, AP_BattMonitor, backend_var_info[4]),
198
#endif
199

200
#if AP_BATT_MONITOR_MAX_INSTANCES > 5
201
    // @Group: 6_
202
    // @Path: AP_BattMonitor_Params.cpp
203
    AP_SUBGROUPINFO(_params[5], "6_", 28, AP_BattMonitor, AP_BattMonitor_Params),
204

205
    // @Group: 6_
206
    // @Path: AP_BattMonitor_Analog.cpp
207
    // @Group: 6_
208
    // @Path: AP_BattMonitor_SMBus.cpp
209
    // @Group: 6_
210
    // @Path: AP_BattMonitor_Sum.cpp
211
    // @Group: 6_
212
    // @Path: AP_BattMonitor_DroneCAN.cpp
213
    // @Group: 6_
214
    // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
215
    // @Group: 6_
216
    // @Path: AP_BattMonitor_Synthetic_Current.cpp
217
    // @Group: 6_
218
    // @Path: AP_BattMonitor_INA2xx.cpp
219
    // @Group: 6_
220
    // @Path: AP_BattMonitor_ESC.cpp
221
    // @Group: 6_
222
    // @Path: AP_BattMonitor_INA239.cpp
223
    // @Group: 6_
224
    // @Path: AP_BattMonitor_INA3221.cpp
225
    // @Group: 6_
226
    // @Path: AP_BattMonitor_AD7091R5.cpp
227
    AP_SUBGROUPVARPTR(drivers[5], "6_", 46, AP_BattMonitor, backend_var_info[5]),
228
#endif
229

230
#if AP_BATT_MONITOR_MAX_INSTANCES > 6
231
    // @Group: 7_
232
    // @Path: AP_BattMonitor_Params.cpp
233
    AP_SUBGROUPINFO(_params[6], "7_", 29, AP_BattMonitor, AP_BattMonitor_Params),
234

235
    // @Group: 7_
236
    // @Path: AP_BattMonitor_Analog.cpp
237
    // @Group: 7_
238
    // @Path: AP_BattMonitor_SMBus.cpp
239
    // @Group: 7_
240
    // @Path: AP_BattMonitor_Sum.cpp
241
    // @Group: 7_
242
    // @Path: AP_BattMonitor_DroneCAN.cpp
243
    // @Group: 7_
244
    // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
245
    // @Group: 7_
246
    // @Path: AP_BattMonitor_Synthetic_Current.cpp
247
    // @Group: 7_
248
    // @Path: AP_BattMonitor_INA2xx.cpp
249
    // @Group: 7_
250
    // @Path: AP_BattMonitor_ESC.cpp
251
    // @Group: 7_
252
    // @Path: AP_BattMonitor_INA239.cpp
253
    // @Group: 7_
254
    // @Path: AP_BattMonitor_INA3221.cpp
255
    // @Group: 7_
256
    // @Path: AP_BattMonitor_AD7091R5.cpp
257
    AP_SUBGROUPVARPTR(drivers[6], "7_", 47, AP_BattMonitor, backend_var_info[6]),
258
#endif
259

260
#if AP_BATT_MONITOR_MAX_INSTANCES > 7
261
    // @Group: 8_
262
    // @Path: AP_BattMonitor_Params.cpp
263
    AP_SUBGROUPINFO(_params[7], "8_", 30, AP_BattMonitor, AP_BattMonitor_Params),
264

265
    // @Group: 8_
266
    // @Path: AP_BattMonitor_Analog.cpp
267
    // @Group: 8_
268
    // @Path: AP_BattMonitor_SMBus.cpp
269
    // @Group: 8_
270
    // @Path: AP_BattMonitor_Sum.cpp
271
    // @Group: 8_
272
    // @Path: AP_BattMonitor_DroneCAN.cpp
273
    // @Group: 8_
274
    // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
275
    // @Group: 8_
276
    // @Path: AP_BattMonitor_Synthetic_Current.cpp
277
    // @Group: 8_
278
    // @Path: AP_BattMonitor_INA2xx.cpp
279
    // @Group: 8_
280
    // @Path: AP_BattMonitor_ESC.cpp
281
    // @Group: 8_
282
    // @Path: AP_BattMonitor_INA239.cpp
283
    // @Group: 8_
284
    // @Path: AP_BattMonitor_INA3221.cpp
285
    // @Group: 8_
286
    // @Path: AP_BattMonitor_AD7091R5.cpp
287
    AP_SUBGROUPVARPTR(drivers[7], "8_", 48, AP_BattMonitor, backend_var_info[7]),
288
#endif
289

290
#if AP_BATT_MONITOR_MAX_INSTANCES > 8
291
    // @Group: 9_
292
    // @Path: AP_BattMonitor_Params.cpp
293
    AP_SUBGROUPINFO(_params[8], "9_", 31, AP_BattMonitor, AP_BattMonitor_Params),
294

295
    // @Group: 9_
296
    // @Path: AP_BattMonitor_Analog.cpp
297
    // @Group: 9_
298
    // @Path: AP_BattMonitor_SMBus.cpp
299
    // @Group: 9_
300
    // @Path: AP_BattMonitor_Sum.cpp
301
    // @Group: 9_
302
    // @Path: AP_BattMonitor_DroneCAN.cpp
303
    // @Group: 9_
304
    // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
305
    // @Group: 9_
306
    // @Path: AP_BattMonitor_Synthetic_Current.cpp
307
    // @Group: 9_
308
    // @Path: AP_BattMonitor_INA2xx.cpp
309
    // @Group: 9_
310
    // @Path: AP_BattMonitor_ESC.cpp
311
    // @Group: 9_
312
    // @Path: AP_BattMonitor_INA239.cpp
313
    // @Group: 9_
314
    // @Path: AP_BattMonitor_INA3221.cpp
315
    // @Group: 9_
316
    // @Path: AP_BattMonitor_AD7091R5.cpp
317
    AP_SUBGROUPVARPTR(drivers[8], "9_", 49, AP_BattMonitor, backend_var_info[8]),
318
#endif
319

320
#if AP_BATT_MONITOR_MAX_INSTANCES > 9
321
    // @Group: A_
322
    // @Path: AP_BattMonitor_Params.cpp
323
    AP_SUBGROUPINFO(_params[9], "A_", 32, AP_BattMonitor, AP_BattMonitor_Params),
324

325
    // @Group: A_
326
    // @Path: AP_BattMonitor_Analog.cpp
327
    // @Group: A_
328
    // @Path: AP_BattMonitor_SMBus.cpp
329
    // @Group: A_
330
    // @Path: AP_BattMonitor_Sum.cpp
331
    // @Group: A_
332
    // @Path: AP_BattMonitor_DroneCAN.cpp
333
    // @Group: A_
334
    // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
335
    // @Group: A_
336
    // @Path: AP_BattMonitor_Synthetic_Current.cpp
337
    // @Group: A_
338
    // @Path: AP_BattMonitor_INA2xx.cpp
339
    // @Group: A_
340
    // @Path: AP_BattMonitor_ESC.cpp
341
    // @Group: A_
342
    // @Path: AP_BattMonitor_INA239.cpp
343
    // @Group: A_
344
    // @Path: AP_BattMonitor_INA3221.cpp
345
    // @Group: A_
346
    // @Path: AP_BattMonitor_AD7091R5.cpp
347
    AP_SUBGROUPVARPTR(drivers[9], "A_", 50, AP_BattMonitor, backend_var_info[9]),
348
#endif
349

350
#if AP_BATT_MONITOR_MAX_INSTANCES > 10
351
    // @Group: B_
352
    // @Path: AP_BattMonitor_Params.cpp
353
    AP_SUBGROUPINFO(_params[10], "B_", 33, AP_BattMonitor, AP_BattMonitor_Params),
354

355
    // @Group: B_
356
    // @Path: AP_BattMonitor_Analog.cpp
357
    // @Group: B_
358
    // @Path: AP_BattMonitor_SMBus.cpp
359
    // @Group: B_
360
    // @Path: AP_BattMonitor_Sum.cpp
361
    // @Group: B_
362
    // @Path: AP_BattMonitor_DroneCAN.cpp
363
    // @Group: B_
364
    // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
365
    // @Group: B_
366
    // @Path: AP_BattMonitor_Synthetic_Current.cpp
367
    // @Group: B_
368
    // @Path: AP_BattMonitor_INA2xx.cpp
369
    // @Group: B_
370
    // @Path: AP_BattMonitor_ESC.cpp
371
    // @Group: B_
372
    // @Path: AP_BattMonitor_INA239.cpp
373
    // @Group: B_
374
    // @Path: AP_BattMonitor_INA3221.cpp
375
    // @Group: B_
376
    // @Path: AP_BattMonitor_AD7091R5.cpp
377
    AP_SUBGROUPVARPTR(drivers[10], "B_", 51, AP_BattMonitor, backend_var_info[10]),
378
#endif
379

380
#if AP_BATT_MONITOR_MAX_INSTANCES > 11
381
    // @Group: C_
382
    // @Path: AP_BattMonitor_Params.cpp
383
    AP_SUBGROUPINFO(_params[11], "C_", 34, AP_BattMonitor, AP_BattMonitor_Params),
384

385
    // @Group: C_
386
    // @Path: AP_BattMonitor_Analog.cpp
387
    // @Group: C_
388
    // @Path: AP_BattMonitor_SMBus.cpp
389
    // @Group: C_
390
    // @Path: AP_BattMonitor_Sum.cpp
391
    // @Group: C_
392
    // @Path: AP_BattMonitor_DroneCAN.cpp
393
    // @Group: C_
394
    // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
395
    // @Group: C_
396
    // @Path: AP_BattMonitor_Synthetic_Current.cpp
397
    // @Group: C_
398
    // @Path: AP_BattMonitor_INA2xx.cpp
399
    // @Group: C_
400
    // @Path: AP_BattMonitor_ESC.cpp
401
    // @Group: C_
402
    // @Path: AP_BattMonitor_INA239.cpp
403
    // @Group: C_
404
    // @Path: AP_BattMonitor_INA3221.cpp
405
    // @Group: C_
406
    // @Path: AP_BattMonitor_AD7091R5.cpp
407
    AP_SUBGROUPVARPTR(drivers[11], "C_", 52, AP_BattMonitor, backend_var_info[11]),
408
#endif
409

410
#if AP_BATT_MONITOR_MAX_INSTANCES > 12
411
    // @Group: D_
412
    // @Path: AP_BattMonitor_Params.cpp
413
    AP_SUBGROUPINFO(_params[12], "D_", 35, AP_BattMonitor, AP_BattMonitor_Params),
414

415
    // @Group: D_
416
    // @Path: AP_BattMonitor_Analog.cpp
417
    // @Group: D_
418
    // @Path: AP_BattMonitor_SMBus.cpp
419
    // @Group: D_
420
    // @Path: AP_BattMonitor_Sum.cpp
421
    // @Group: D_
422
    // @Path: AP_BattMonitor_DroneCAN.cpp
423
    // @Group: D_
424
    // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
425
    // @Group: D_
426
    // @Path: AP_BattMonitor_Synthetic_Current.cpp
427
    // @Group: D_
428
    // @Path: AP_BattMonitor_INA2xx.cpp
429
    // @Group: D_
430
    // @Path: AP_BattMonitor_ESC.cpp
431
    // @Group: D_
432
    // @Path: AP_BattMonitor_INA239.cpp
433
    // @Group: D_
434
    // @Path: AP_BattMonitor_INA3221.cpp
435
    // @Group: D_
436
    // @Path: AP_BattMonitor_AD7091R5.cpp
437
    AP_SUBGROUPVARPTR(drivers[12], "D_", 53, AP_BattMonitor, backend_var_info[12]),
438
#endif
439

440
#if AP_BATT_MONITOR_MAX_INSTANCES > 13
441
    // @Group: E_
442
    // @Path: AP_BattMonitor_Params.cpp
443
    AP_SUBGROUPINFO(_params[13], "E_", 36, AP_BattMonitor, AP_BattMonitor_Params),
444

445
    // @Group: E_
446
    // @Path: AP_BattMonitor_Analog.cpp
447
    // @Group: E_
448
    // @Path: AP_BattMonitor_SMBus.cpp
449
    // @Group: E_
450
    // @Path: AP_BattMonitor_Sum.cpp
451
    // @Group: E_
452
    // @Path: AP_BattMonitor_DroneCAN.cpp
453
    // @Group: E_
454
    // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
455
    // @Group: E_
456
    // @Path: AP_BattMonitor_Synthetic_Current.cpp
457
    // @Group: E_
458
    // @Path: AP_BattMonitor_INA2xx.cpp
459
    // @Group: E_
460
    // @Path: AP_BattMonitor_ESC.cpp
461
    // @Group: E_
462
    // @Path: AP_BattMonitor_INA239.cpp
463
    // @Group: E_
464
    // @Path: AP_BattMonitor_INA3221.cpp
465
    // @Group: E_
466
    // @Path: AP_BattMonitor_AD7091R5.cpp
467
    AP_SUBGROUPVARPTR(drivers[13], "E_", 54, AP_BattMonitor, backend_var_info[13]),
468
#endif
469

470
#if AP_BATT_MONITOR_MAX_INSTANCES > 14
471
    // @Group: F_
472
    // @Path: AP_BattMonitor_Params.cpp
473
    AP_SUBGROUPINFO(_params[14], "F_", 37, AP_BattMonitor, AP_BattMonitor_Params),
474

475
    // @Group: F_
476
    // @Path: AP_BattMonitor_Analog.cpp
477
    // @Group: F_
478
    // @Path: AP_BattMonitor_SMBus.cpp
479
    // @Group: F_
480
    // @Path: AP_BattMonitor_Sum.cpp
481
    // @Group: F_
482
    // @Path: AP_BattMonitor_DroneCAN.cpp
483
    // @Group: F_
484
    // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
485
    // @Group: F_
486
    // @Path: AP_BattMonitor_Synthetic_Current.cpp
487
    // @Group: F_
488
    // @Path: AP_BattMonitor_INA2xx.cpp
489
    // @Group: F_
490
    // @Path: AP_BattMonitor_ESC.cpp
491
    // @Group: F_
492
    // @Path: AP_BattMonitor_INA239.cpp
493
    // @Group: F_
494
    // @Path: AP_BattMonitor_INA3221.cpp
495
    // @Group: F_
496
    // @Path: AP_BattMonitor_AD7091R5.cpp
497
    AP_SUBGROUPVARPTR(drivers[14], "F_", 55, AP_BattMonitor, backend_var_info[14]),
498
#endif
499

500
#if AP_BATT_MONITOR_MAX_INSTANCES > 15
501
    // @Group: G_
502
    // @Path: AP_BattMonitor_Params.cpp
503
    AP_SUBGROUPINFO(_params[15], "G_", 38, AP_BattMonitor, AP_BattMonitor_Params),
504

505
    // @Group: G_
506
    // @Path: AP_BattMonitor_Analog.cpp
507
    // @Group: G_
508
    // @Path: AP_BattMonitor_SMBus.cpp
509
    // @Group: G_
510
    // @Path: AP_BattMonitor_Sum.cpp
511
    // @Group: G_
512
    // @Path: AP_BattMonitor_DroneCAN.cpp
513
    // @Group: G_
514
    // @Path: AP_BattMonitor_FuelLevel_Analog.cpp
515
    // @Group: G_
516
    // @Path: AP_BattMonitor_Synthetic_Current.cpp
517
    // @Group: G_
518
    // @Path: AP_BattMonitor_INA2xx.cpp
519
    // @Group: G_
520
    // @Path: AP_BattMonitor_ESC.cpp
521
    // @Group: G_
522
    // @Path: AP_BattMonitor_INA239.cpp
523
    // @Group: G_
524
    // @Path: AP_BattMonitor_INA3221.cpp
525
    // @Group: G_
526
    // @Path: AP_BattMonitor_AD7091R5.cpp
527
    AP_SUBGROUPVARPTR(drivers[15], "G_", 56, AP_BattMonitor, backend_var_info[15]),
528
#endif
529

530
#if AP_BATT_MONITOR_MAX_INSTANCES > 16
531
    #error "AP_BATT_MONITOR_MAX_INSTANCES too large, reset_remaining_mask() will cause an assert above 16"
532
#endif
533

534
    AP_GROUPEND
535
};
536

537
const AP_Param::GroupInfo *AP_BattMonitor::backend_var_info[AP_BATT_MONITOR_MAX_INSTANCES];
538

539
// Default constructor.
540
// Note that the Vector/Matrix constructors already implicitly zero
541
// their values.
542
//
543
AP_BattMonitor::AP_BattMonitor(uint32_t log_battery_bit, battery_failsafe_handler_fn_t battery_failsafe_handler_fn, const int8_t *failsafe_priorities) :
544
    _log_battery_bit(log_battery_bit),
545
    _battery_failsafe_handler_fn(battery_failsafe_handler_fn),
546
    _failsafe_priorities(failsafe_priorities)
547
{
548
    AP_Param::setup_object_defaults(this, var_info);
549

550
    if (_singleton != nullptr) {
551
        AP_HAL::panic("AP_BattMonitor must be singleton");
552
    }
553
    _singleton = this;
554
}
555

556
// init - instantiate the battery monitors
557
void
558
AP_BattMonitor::init()
559
{
560
    // check init has not been called before
561
    if (_num_instances != 0) {
562
        return;
563
    }
564

565
    _highest_failsafe_priority = INT8_MAX;
566

567
#ifdef HAL_BATT_MONITOR_DEFAULT
568
    _params[0]._type.set_default(int8_t(HAL_BATT_MONITOR_DEFAULT));
569
#endif
570
#ifdef HAL_BATT2_MONITOR_DEFAULT
571
    _params[1]._type.set_default(int8_t(HAL_BATT2_MONITOR_DEFAULT));
572
#endif
573

574
    // create each instance
575
    for (uint8_t instance=0; instance<AP_BATT_MONITOR_MAX_INSTANCES; instance++) {
576
        // clear out the cell voltages
577
        memset(&state[instance].cell_voltages, 0xFF, sizeof(cells));
578
        state[instance].instance = instance;
579

580
        const auto allocation_type = configured_type(instance);
581
        switch (allocation_type) {
582
#if AP_BATTERY_ANALOG_ENABLED
583
            case Type::ANALOG_VOLTAGE_ONLY:
584
            case Type::ANALOG_VOLTAGE_AND_CURRENT:
585
                drivers[instance] = NEW_NOTHROW AP_BattMonitor_Analog(*this, state[instance], _params[instance]);
586
                break;
587
#endif
588
#if AP_BATTERY_SMBUS_SOLO_ENABLED
589
            case Type::SOLO:
590
                drivers[instance] = NEW_NOTHROW AP_BattMonitor_SMBus_Solo(*this, state[instance], _params[instance]);
591
                break;
592
#endif
593
#if AP_BATTERY_SMBUS_GENERIC_ENABLED
594
            case Type::SMBus_Generic:
595
                drivers[instance] = NEW_NOTHROW AP_BattMonitor_SMBus_Generic(*this, state[instance], _params[instance]);
596
                break;
597
#endif
598
#if AP_BATTERY_SMBUS_SUI_ENABLED
599
            case Type::SUI3:
600
                drivers[instance] = NEW_NOTHROW AP_BattMonitor_SMBus_SUI(*this, state[instance], _params[instance], 3);
601
                break;
602
            case Type::SUI6:
603
                drivers[instance] = NEW_NOTHROW AP_BattMonitor_SMBus_SUI(*this, state[instance], _params[instance], 6);
604
                break;
605
#endif
606
#if AP_BATTERY_SMBUS_MAXELL_ENABLED
607
            case Type::MAXELL:
608
                drivers[instance] = NEW_NOTHROW AP_BattMonitor_SMBus_Maxell(*this, state[instance], _params[instance]);
609
                break;
610
#endif
611
#if AP_BATTERY_SMBUS_ROTOYE_ENABLED
612
            case Type::Rotoye:
613
                drivers[instance] = NEW_NOTHROW AP_BattMonitor_SMBus_Rotoye(*this, state[instance], _params[instance]);
614
                break;
615
#endif
616
#if AP_BATTERY_SMBUS_NEODESIGN_ENABLED
617
            case Type::NeoDesign:
618
                drivers[instance] = NEW_NOTHROW AP_BattMonitor_SMBus_NeoDesign(*this, state[instance], _params[instance]);
619
                break;
620
#endif
621
#if AP_BATTERY_BEBOP_ENABLED
622
            case Type::BEBOP:
623
                drivers[instance] = NEW_NOTHROW AP_BattMonitor_Bebop(*this, state[instance], _params[instance]);
624
                break;
625
#endif
626
#if AP_BATTERY_UAVCAN_BATTERYINFO_ENABLED
627
            case Type::UAVCAN_BatteryInfo:
628
                drivers[instance] = NEW_NOTHROW AP_BattMonitor_DroneCAN(*this, state[instance], AP_BattMonitor_DroneCAN::UAVCAN_BATTERY_INFO, _params[instance]);
629
                break;
630
#endif
631
#if AP_BATTERY_ESC_ENABLED
632
            case Type::BLHeliESC:
633
                drivers[instance] = NEW_NOTHROW AP_BattMonitor_ESC(*this, state[instance], _params[instance]);
634
                break;
635
#endif
636
#if AP_BATTERY_SUM_ENABLED
637
            case Type::Sum:
638
                drivers[instance] = NEW_NOTHROW AP_BattMonitor_Sum(*this, state[instance], _params[instance], instance);
639
                break;
640
#endif
641
#if AP_BATTERY_FUELFLOW_ENABLED
642
            case Type::FuelFlow:
643
                drivers[instance] = NEW_NOTHROW AP_BattMonitor_FuelFlow(*this, state[instance], _params[instance]);
644
                break;
645
#endif // AP_BATTERY_FUELFLOW_ENABLED
646
#if AP_BATTERY_FUELLEVEL_PWM_ENABLED
647
            case Type::FuelLevel_PWM:
648
                drivers[instance] = NEW_NOTHROW AP_BattMonitor_FuelLevel_PWM(*this, state[instance], _params[instance]);
649
                break;
650
#endif // AP_BATTERY_FUELLEVEL_PWM_ENABLED
651
#if AP_BATTERY_FUELLEVEL_ANALOG_ENABLED
652
            case Type::FuelLevel_Analog:
653
                drivers[instance] = NEW_NOTHROW AP_BattMonitor_FuelLevel_Analog(*this, state[instance], _params[instance]);
654
                break;
655
#endif // AP_BATTERY_FUELLEVEL_ANALOG_ENABLED
656
#if HAL_GENERATOR_ENABLED
657
            case Type::GENERATOR_ELEC:
658
                drivers[instance] = NEW_NOTHROW AP_BattMonitor_Generator_Elec(*this, state[instance], _params[instance]);
659
                break;
660
            case Type::GENERATOR_FUEL:
661
                drivers[instance] = NEW_NOTHROW AP_BattMonitor_Generator_FuelLevel(*this, state[instance], _params[instance]);
662
                break;
663
#endif // HAL_GENERATOR_ENABLED
664
#if AP_BATTERY_INA2XX_ENABLED
665
            case Type::INA2XX:
666
                drivers[instance] = NEW_NOTHROW AP_BattMonitor_INA2XX(*this, state[instance], _params[instance]);
667
                break;
668
#endif
669
#if AP_BATTERY_LTC2946_ENABLED
670
            case Type::LTC2946:
671
                drivers[instance] = NEW_NOTHROW AP_BattMonitor_LTC2946(*this, state[instance], _params[instance]);
672
                break;
673
#endif
674
#if HAL_TORQEEDO_ENABLED
675
            case Type::Torqeedo:
676
                drivers[instance] = NEW_NOTHROW AP_BattMonitor_Torqeedo(*this, state[instance], _params[instance]);
677
                break;
678
#endif
679
#if AP_BATTERY_SYNTHETIC_CURRENT_ENABLED
680
            case Type::Analog_Volt_Synthetic_Current:
681
                drivers[instance] = NEW_NOTHROW AP_BattMonitor_Synthetic_Current(*this, state[instance], _params[instance]);
682
                break;
683
#endif
684
#if AP_BATTERY_INA239_ENABLED
685
            case Type::INA239_SPI:
686
                drivers[instance] = NEW_NOTHROW AP_BattMonitor_INA239(*this, state[instance], _params[instance]);
687
                break;
688
#endif
689
#if AP_BATTERY_EFI_ENABLED
690
            case Type::EFI:
691
                drivers[instance] = NEW_NOTHROW AP_BattMonitor_EFI(*this, state[instance], _params[instance]);
692
                break;
693
#endif // AP_BATTERY_EFI_ENABLED
694
#if AP_BATTERY_AD7091R5_ENABLED
695
            case Type::AD7091R5:
696
                drivers[instance] = NEW_NOTHROW AP_BattMonitor_AD7091R5(*this, state[instance], _params[instance]);
697
                break;
698
#endif// AP_BATTERY_AD7091R5_ENABLED
699
#if AP_BATTERY_SCRIPTING_ENABLED
700
            case Type::Scripting:
701
                drivers[instance] = NEW_NOTHROW AP_BattMonitor_Scripting(*this, state[instance], _params[instance]);
702
                break;
703
#endif // AP_BATTERY_SCRIPTING_ENABLED
704
#if AP_BATTERY_INA3221_ENABLED
705
            case Type::INA3221:
706
                drivers[instance] = NEW_NOTHROW AP_BattMonitor_INA3221(*this, state[instance], _params[instance]);
707
                break;
708
#endif  // AP_BATTERY_INA3221_ENABLED
709
            case Type::NONE:
710
            default:
711
                break;
712
        }
713
        if (drivers[instance] != nullptr) {
714
            state[instance].type = allocation_type;
715
        }
716
    // if the backend has some local parameters then make those available in the tree
717
    if (drivers[instance] && state[instance].var_info) {
718
        backend_var_info[instance] = state[instance].var_info;
719
        AP_Param::load_object_from_eeprom(drivers[instance], backend_var_info[instance]);
720

721
        // param count could have changed
722
        AP_Param::invalidate_count();
723
    }
724

725
        // call init function for each backend
726
        if (drivers[instance] != nullptr) {
727
            drivers[instance]->init();
728
            // _num_instances is actually the index for looping over instances
729
            // the user may have BATT_MONITOR=0 and BATT2_MONITOR=7, in which case
730
            // there will be a gap, but as we always check for drivers[instances] being nullptr
731
            // this is safe
732
            _num_instances = instance + 1;
733

734
            // Convert the old analog & Bus parameters to the new dynamic parameter groups
735
            convert_dynamic_param_groups(instance);
736
        }
737
    }
738
}
739

740
void AP_BattMonitor::convert_dynamic_param_groups(uint8_t instance)
741
{
742
    AP_Param::ConversionInfo info;
743
    if (!AP_Param::find_top_level_key_by_pointer(this, info.old_key)) {
744
        return;
745
    }
746

747
    char param_prefix[6] {};
748
    char param_name[17] {};
749
    info.new_name = param_name;
750

751
    const uint8_t param_instance = instance + 1;
752
    // first battmonitor does not have '1' in the param name
753
    if(param_instance == 1) {
754
        hal.util->snprintf(param_prefix, sizeof(param_prefix), "BATT");
755
    } else {
756
        hal.util->snprintf(param_prefix, sizeof(param_prefix), "BATT%X", param_instance);
757
    }
758
    param_prefix[sizeof(param_prefix)-1] = '\0';
759

760
    hal.util->snprintf(param_name, sizeof(param_name), "%s_%s", param_prefix, "MONITOR");
761
    param_name[sizeof(param_name)-1] = '\0';
762

763
    // Find the index of the BATTn_MONITOR which is not moving to index the moving parameters off from
764
    AP_Param::ParamToken token = AP_Param::ParamToken {};
765
    ap_var_type type;
766
    AP_Param* param = AP_Param::find_by_name(param_name, &type, &token);
767
    const uint8_t battmonitor_index = 1;
768
    if( param == nullptr) {
769
        // BATTn_MONITOR not found
770
        return;
771
    }
772

773
    const struct convert_table {
774
        uint32_t old_group_element;
775
        ap_var_type type;
776
        const char* new_name;
777
    }  conversion_table[] = {
778
        // PARAMETER_CONVERSION - Added: Aug-2021
779
            { 2,  AP_PARAM_INT8,  "VOLT_PIN"  },
780
            { 3,  AP_PARAM_INT8,  "CURR_PIN"  },
781
            { 4,  AP_PARAM_FLOAT, "VOLT_MULT" },
782
            { 5,  AP_PARAM_FLOAT, "AMP_PERVLT"},
783
            { 6,  AP_PARAM_FLOAT, "AMP_OFFSET"},
784
            { 20, AP_PARAM_INT8,  "I2C_BUS"   },
785
        };
786

787
    for (const auto & elem : conversion_table) {
788
        info.old_group_element = token.group_element + ((elem.old_group_element - battmonitor_index) * 64);
789
        info.type = elem.type;
790

791
        hal.util->snprintf(param_name, sizeof(param_name), "%s_%s", param_prefix, elem.new_name);
792
        AP_Param::convert_old_parameter(&info, 1.0f, 0);
793
    }
794
}
795

796
// read - For all active instances read voltage & current; log BAT, BCL, POWR, MCU
797
void AP_BattMonitor::read()
798
{
799
#if HAL_LOGGING_ENABLED
800
    AP_Logger *logger = AP_Logger::get_singleton();
801
    if (logger != nullptr && logger->should_log(_log_battery_bit)) {
802
        logger->Write_Power();
803
    }
804
#endif
805

806
    const uint32_t now_ms = AP_HAL::millis();
807
    for (uint8_t i=0; i<_num_instances; i++) {
808
            if (drivers[i] == nullptr) {
809
                continue;
810
            }
811
            if (allocated_type(i) != configured_type(i)) {
812
                continue;
813
            }
814
            // allow run-time disabling; this is technically redundant
815
            if (configured_type(i) == Type::NONE) {
816
                continue;
817
            }
818
            drivers[i]->read();
819
            drivers[i]->update_resistance_estimate();
820

821
#if AP_BATTERY_ESC_TELEM_OUTBOUND_ENABLED
822
            drivers[i]->update_esc_telem_outbound();
823
#endif
824

825
            // Update last heathy timestamp
826
            if (state[i].healthy) {
827
                state[i].last_healthy_ms = now_ms;
828
            }
829

830
#if HAL_LOGGING_ENABLED
831
            if (logger != nullptr && logger->should_log(_log_battery_bit)) {
832
                const uint64_t time_us = AP_HAL::micros64();
833
                drivers[i]->Log_Write_BAT(i, time_us);
834
                drivers[i]->Log_Write_BCL(i, time_us);
835
            }
836
#endif
837
    }
838

839
    check_failsafes();
840
    
841
    checkPoweringOff();
842
}
843

844
// healthy - returns true if monitor is functioning
845
bool AP_BattMonitor::healthy(uint8_t instance) const {
846
    return instance < _num_instances && state[instance].healthy;
847
}
848

849
/// voltage - returns battery voltage in volts
850
float AP_BattMonitor::voltage(uint8_t instance) const
851
{
852
    if (instance < _num_instances) {
853
        return state[instance].voltage;
854
    } else {
855
        return 0.0f;
856
    }
857
}
858

859
/// get voltage with sag removed (based on battery current draw and resistance)
860
/// this will always be greater than or equal to the raw voltage
861
float AP_BattMonitor::voltage_resting_estimate(uint8_t instance) const
862
{
863
    if (instance < _num_instances && drivers[instance] != nullptr) {
864
        return drivers[instance]->voltage_resting_estimate();
865
    } else {
866
        return 0.0f;
867
    }
868
}
869

870
/// voltage - returns battery voltage in volts for GCS, may be resting voltage if option enabled
871
float AP_BattMonitor::gcs_voltage(uint8_t instance) const
872
{
873
    if (instance >= _num_instances || drivers[instance] == nullptr) {
874
        return 0.0f;
875
    }
876
    if (drivers[instance]->option_is_set(AP_BattMonitor_Params::Options::GCS_Resting_Voltage)) {
877
        return voltage_resting_estimate(instance);
878
    }
879
    return state[instance].voltage;
880
}
881

882
bool AP_BattMonitor::option_is_set(uint8_t instance, AP_BattMonitor_Params::Options option) const
883
{
884
    if (instance >= _num_instances || drivers[instance] == nullptr) {
885
        return false;
886
    }
887
    return drivers[instance]->option_is_set(option);
888
}
889

890
/// current_amps - returns the instantaneous current draw in amperes
891
bool AP_BattMonitor::current_amps(float &current, uint8_t instance) const {
892
    if ((instance < _num_instances) && (drivers[instance] != nullptr) && drivers[instance]->has_current()) {
893
        current = state[instance].current_amps;
894
        return true;
895
    } else {
896
        return false;
897
    }
898
}
899

900
/// consumed_mah - returns total current drawn since start-up in milliampere.hours
901
bool AP_BattMonitor::consumed_mah(float &mah, const uint8_t instance) const {
902
    if ((instance < _num_instances) && (drivers[instance] != nullptr) && drivers[instance]->has_current()) {
903
        const float consumed_mah = state[instance].consumed_mah;
904
        if (isnan(consumed_mah)) {
905
            return false;
906
        }
907
        mah = consumed_mah;
908
        return true;
909
    } else {
910
        return false;
911
    }
912
}
913

914
/// consumed_wh - returns energy consumed since start-up in Watt.hours
915
bool AP_BattMonitor::consumed_wh(float &wh, const uint8_t instance) const {
916
    if (instance < _num_instances && drivers[instance] != nullptr && drivers[instance]->has_consumed_energy()) {
917
        wh = state[instance].consumed_wh;
918
        return true;
919
    } else {
920
        return false;
921
    }
922
}
923

924
/// capacity_remaining_pct - returns true if the percentage is valid and writes to percentage argument
925
bool AP_BattMonitor::capacity_remaining_pct(uint8_t &percentage, uint8_t instance) const
926
{
927
    if (instance < _num_instances && drivers[instance] != nullptr) {
928
        return drivers[instance]->capacity_remaining_pct(percentage);
929
    }
930
    return false;
931
}
932

933
/// time_remaining - returns remaining battery time
934
bool AP_BattMonitor::time_remaining(uint32_t &seconds, uint8_t instance) const
935
{
936
    if (instance < _num_instances && drivers[instance] != nullptr && state[instance].has_time_remaining) {
937
        seconds = state[instance].time_remaining;
938
        return true;
939
    }
940
    return false;
941
}
942

943
/// pack_capacity_mah - returns the capacity of the battery pack in mAh when the pack is full
944
int32_t AP_BattMonitor::pack_capacity_mah(uint8_t instance) const
945
{
946
    if (instance < AP_BATT_MONITOR_MAX_INSTANCES) {
947
        return _params[instance]._pack_capacity;
948
    } else {
949
        return 0;
950
    }
951
}
952

953
void AP_BattMonitor::check_failsafes(void)
954
{
955
    if (hal.util->get_soft_armed()) {
956
        for (uint8_t i = 0; i < _num_instances; i++) {
957
            if (drivers[i] == nullptr) {
958
                continue;
959
            }
960

961
            const Failsafe type = drivers[i]->update_failsafes();
962
            if (type <= state[i].failsafe) {
963
                continue;
964
            }
965

966
            int8_t action = 0;
967
            const char *type_str = nullptr;
968
            switch (type) {
969
                case Failsafe::None:
970
                    continue; // should not have been called in this case
971
                case Failsafe::Unhealthy:
972
                    // Report only for unhealthy, could add action param in the future
973
                    action = 0;
974
                    type_str = "missing, last:";
975
                    break;
976
                case Failsafe::Low:
977
                    action = _params[i]._failsafe_low_action;
978
                    type_str = "low";
979
                    break;
980
                case Failsafe::Critical:
981
                    action = _params[i]._failsafe_critical_action;
982
                    type_str = "critical";
983
                    break;
984
            }
985

986
            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Battery %d is %s %.2fV used %.0f mAh", i + 1, type_str,
987
                            (double)voltage(i), (double)state[i].consumed_mah);
988
            _has_triggered_failsafe = true;
989
#ifndef HAL_BUILD_AP_PERIPH
990
            AP_Notify::flags.failsafe_battery = true;
991
#endif
992
            state[i].failsafe = type;
993

994
            // map the desired failsafe action to a priority level
995
            int8_t priority = 0;
996
            if (_failsafe_priorities != nullptr) {
997
                while (_failsafe_priorities[priority] != -1) {
998
                    if (_failsafe_priorities[priority] == action) {
999
                        break;
1000
                    }
1001
                    priority++;
1002
                }
1003

1004
            }
1005

1006
            // trigger failsafe if the action was equal or higher priority
1007
            // It's valid to retrigger the same action if a different battery provoked the event
1008
            if (priority <= _highest_failsafe_priority) {
1009
                _battery_failsafe_handler_fn(type_str, action);
1010
                _highest_failsafe_priority = priority;
1011
            }
1012
        }
1013
    }
1014
}
1015

1016
// return true if any battery is pushing too much power
1017
bool AP_BattMonitor::overpower_detected() const
1018
{
1019
#if AP_BATTERY_WATT_MAX_ENABLED && APM_BUILD_TYPE(APM_BUILD_ArduPlane)
1020
    for (uint8_t instance = 0; instance < _num_instances; instance++) {
1021
        if (overpower_detected(instance)) {
1022
            return true;
1023
        }
1024
    }
1025
#endif
1026
    return false;
1027
}
1028

1029
bool AP_BattMonitor::overpower_detected(uint8_t instance) const
1030
{
1031
#if AP_BATTERY_WATT_MAX_ENABLED && APM_BUILD_TYPE(APM_BUILD_ArduPlane)
1032
    if (instance < _num_instances && _params[instance]._watt_max > 0) {
1033
        const float power = state[instance].current_amps * state[instance].voltage;
1034
        return state[instance].healthy && (power > _params[instance]._watt_max);
1035
    }
1036
#endif
1037
    return false;
1038
}
1039

1040
bool AP_BattMonitor::has_cell_voltages(const uint8_t instance) const
1041
{
1042
    if (instance < _num_instances && drivers[instance] != nullptr) {
1043
        return drivers[instance]->has_cell_voltages();
1044
    }
1045

1046
    return false;
1047
}
1048

1049
// return the current cell voltages, returns the first monitor instances cells if the instance is out of range
1050
const AP_BattMonitor::cells & AP_BattMonitor::get_cell_voltages(const uint8_t instance) const
1051
{
1052
    if (instance >= AP_BATT_MONITOR_MAX_INSTANCES) {
1053
        return state[AP_BATT_PRIMARY_INSTANCE].cell_voltages;
1054
    } else {
1055
        return state[instance].cell_voltages;
1056
    }
1057
}
1058

1059
// get once cell voltage (for scripting)
1060
bool AP_BattMonitor::get_cell_voltage(uint8_t instance, uint8_t cell, float &voltage) const
1061
{
1062
    if (!has_cell_voltages(instance) ||
1063
        cell >= AP_BATT_MONITOR_CELLS_MAX) {
1064
        return false;
1065
    }
1066
    const auto &cell_voltages = get_cell_voltages(instance);
1067
    const uint16_t voltage_mv = cell_voltages.cells[cell];
1068
    if (voltage_mv == 0 || voltage_mv == UINT16_MAX) {
1069
        // UINT16_MAX is used as invalid indicator
1070
        return false;
1071
    }
1072
    voltage = voltage_mv*0.001;
1073
    return true;
1074
}
1075

1076
// returns true if there is a temperature reading
1077
bool AP_BattMonitor::get_temperature(float &temperature, const uint8_t instance) const
1078
{
1079
    if (instance >= _num_instances || drivers[instance] == nullptr) {
1080
        return false;
1081
    }
1082

1083
    return drivers[instance]->get_temperature(temperature);
1084
}
1085

1086
#if AP_TEMPERATURE_SENSOR_ENABLED
1087
// return true when successfully setting a battery temperature from an external source by instance
1088
bool AP_BattMonitor::set_temperature(const float temperature, const uint8_t instance)
1089
{
1090
    if (instance >= _num_instances || drivers[instance] == nullptr) {
1091
        return false;
1092
    }
1093
    state[instance].temperature_external = temperature;
1094
    state[instance].temperature_external_use = true;
1095
    return true;
1096
}
1097

1098
// return true when successfully setting a battery temperature from an external source by serial_number
1099
bool AP_BattMonitor::set_temperature_by_serial_number(const float temperature, const int32_t serial_number)
1100
{
1101
    bool success = false;
1102
    for (uint8_t i = 0; i < _num_instances; i++) {
1103
        if (drivers[i] != nullptr && get_serial_number(i) == serial_number) {
1104
            success |= set_temperature(temperature, i);
1105
        }
1106
    }
1107
    return success;
1108
}
1109
#endif // AP_TEMPERATURE_SENSOR_ENABLED
1110

1111
// return true if cycle count can be provided and fills in cycles argument
1112
bool AP_BattMonitor::get_cycle_count(uint8_t instance, uint16_t &cycles) const
1113
{
1114
    if (instance >= _num_instances || (drivers[instance] == nullptr)) {
1115
        return false;
1116
    }
1117
    return drivers[instance]->get_cycle_count(cycles);
1118
}
1119

1120
bool AP_BattMonitor::arming_checks(size_t buflen, char *buffer) const
1121
{
1122
    char temp_buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1] {};
1123

1124
    for (uint8_t i = 0; i < AP_BATT_MONITOR_MAX_INSTANCES; i++) {
1125
        const auto expected_type = configured_type(i);
1126

1127
        if (drivers[i] == nullptr && expected_type == Type::NONE) {
1128
            continue;
1129
        }
1130

1131
#if !AP_BATTERY_SUM_ENABLED
1132
        // CONVERSION - Added Sep 2024 for ArduPilot 4.6 as we are
1133
        // removing the SUM backend on 1MB boards.  Give a
1134
        // more-specific error for the sum backend:
1135
        if (expected_type == Type::Sum) {
1136
            hal.util->snprintf(buffer, buflen, "Battery %d %s", i + 1, "feature BATTERY_SUM not available");
1137
            return false;
1138
        }
1139
#endif
1140

1141
        if (drivers[i] == nullptr || allocated_type(i) != expected_type) {
1142
            hal.util->snprintf(buffer, buflen, "Battery %d %s", i + 1, "unhealthy");
1143
            return false;
1144
        }
1145

1146
        if (!drivers[i]->arming_checks(temp_buffer, sizeof(temp_buffer))) {
1147
            hal.util->snprintf(buffer, buflen, "Battery %d %s", i + 1, temp_buffer);
1148
            return false;
1149
        }
1150
    }
1151

1152
    return true;
1153
}
1154

1155
// Check's each smart battery instance for its powering off state and broadcasts notifications
1156
void AP_BattMonitor::checkPoweringOff(void)
1157
{
1158
    for (uint8_t i = 0; i < _num_instances; i++) {
1159
        if (state[i].is_powering_off && !state[i].powerOffNotified) {
1160
#ifndef HAL_BUILD_AP_PERIPH
1161
            // Set the AP_Notify flag, which plays the power off tones
1162
            AP_Notify::flags.powering_off = true;
1163
#endif
1164

1165
            // Send a Mavlink broadcast announcing the shutdown
1166
#if HAL_GCS_ENABLED
1167
            mavlink_command_long_t cmd_msg{};
1168
            cmd_msg.command = MAV_CMD_POWER_OFF_INITIATED;
1169
            cmd_msg.param1 = i+1;
1170
            GCS_MAVLINK::send_to_components(MAVLINK_MSG_ID_COMMAND_LONG, (char*)&cmd_msg, sizeof(cmd_msg));
1171
            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Vehicle %d battery %d is powering off", mavlink_system.sysid, i+1);
1172
#endif
1173

1174
            // only send this once
1175
            state[i].powerOffNotified = true;
1176
        }
1177
    }
1178
}
1179

1180
/*
1181
  reset battery remaining percentage for batteries that integrate to
1182
  calculate percentage remaining
1183
*/
1184
bool AP_BattMonitor::reset_remaining_mask(uint16_t battery_mask, float percentage)
1185
{
1186
    static_assert(AP_BATT_MONITOR_MAX_INSTANCES <= 16, "More batteries are enabled then can be reset");
1187
    bool ret = true;
1188
    Failsafe highest_failsafe = Failsafe::None;
1189
    for (uint8_t i = 0; i < _num_instances; i++) {
1190
        if ((1U<<i) & battery_mask) {
1191
            if (drivers[i] != nullptr) {
1192
                ret &= drivers[i]->reset_remaining(percentage);
1193
            } else {
1194
                ret = false;
1195
            }
1196
        }
1197
        if (state[i].failsafe > highest_failsafe) {
1198
            highest_failsafe = state[i].failsafe;
1199
        }
1200
    }
1201

1202
    // If all backends are not in failsafe then set overall failsafe state
1203
    if (highest_failsafe == Failsafe::None) {
1204
        _highest_failsafe_priority = INT8_MAX;
1205
        _has_triggered_failsafe = false;
1206
        // and reset notify flag
1207
        AP_Notify::flags.failsafe_battery = false;
1208
    }
1209
    return ret;
1210
}
1211

1212
// Returns the mavlink charge state. The following mavlink charge states are not used
1213
// MAV_BATTERY_CHARGE_STATE_EMERGENCY , MAV_BATTERY_CHARGE_STATE_FAILED
1214
// MAV_BATTERY_CHARGE_STATE_UNHEALTHY, MAV_BATTERY_CHARGE_STATE_CHARGING
1215
MAV_BATTERY_CHARGE_STATE AP_BattMonitor::get_mavlink_charge_state(const uint8_t instance) const 
1216
{
1217
    if (instance >= _num_instances) {
1218
        return MAV_BATTERY_CHARGE_STATE_UNDEFINED;
1219
    }
1220

1221
    switch (state[instance].failsafe) {
1222

1223
    case Failsafe::None:
1224
    case Failsafe::Unhealthy:
1225
        if (get_mavlink_fault_bitmask(instance) != 0 || !healthy()) {
1226
            return MAV_BATTERY_CHARGE_STATE_UNHEALTHY;
1227
        }
1228
        return MAV_BATTERY_CHARGE_STATE_OK;
1229

1230
    case Failsafe::Low:
1231
        return MAV_BATTERY_CHARGE_STATE_LOW;
1232

1233
    case Failsafe::Critical:
1234
        return MAV_BATTERY_CHARGE_STATE_CRITICAL;
1235
    }
1236

1237
    // Should not reach this
1238
    return MAV_BATTERY_CHARGE_STATE_UNDEFINED;
1239
}
1240

1241
// Returns mavlink fault state
1242
uint32_t AP_BattMonitor::get_mavlink_fault_bitmask(const uint8_t instance) const
1243
{
1244
    if (instance >= _num_instances || drivers[instance] == nullptr) {
1245
        return 0;
1246
    }
1247
    return drivers[instance]->get_mavlink_fault_bitmask();
1248
}
1249

1250
// return true if state of health (as a percentage) can be provided and fills in soh_pct argument
1251
bool AP_BattMonitor::get_state_of_health_pct(uint8_t instance, uint8_t &soh_pct) const
1252
{
1253
    if (instance >= _num_instances || drivers[instance] == nullptr) {
1254
        return false;
1255
    }
1256
    return drivers[instance]->get_state_of_health_pct(soh_pct);
1257
}
1258

1259
// Enable/Disable (Turn on/off) MPPT power to all backends who are MPPTs
1260
void AP_BattMonitor::MPPT_set_powered_state_to_all(const bool power_on)
1261
{
1262
    for (uint8_t i=0; i < _num_instances; i++) {
1263
        MPPT_set_powered_state(i, power_on);
1264
    }
1265
}
1266

1267
// Enable/Disable (Turn on/off) MPPT power. When disabled, the MPPT does not
1268
// supply energy to the system regardless if it's capable to or not. When enabled
1269
// it will supply energy if available.
1270
void AP_BattMonitor::MPPT_set_powered_state(const uint8_t instance, const bool power_on)
1271
{
1272
    if (instance < _num_instances) {
1273
        drivers[instance]->mppt_set_powered_state(power_on);
1274
    }
1275
}
1276

1277
/*
1278
  check that all configured battery monitors are healthy
1279
 */
1280
bool AP_BattMonitor::healthy() const
1281
{
1282
    for (uint8_t i=0; i< _num_instances; i++) {
1283
        if (allocated_type(i) != configured_type(i)) {
1284
            return false;
1285
        }
1286
        // allow run-time disabling; this is technically redundant
1287
        if (configured_type(i) == Type::NONE) {
1288
            continue;
1289
        }
1290
        if (!healthy(i)) {
1291
            return false;
1292
        }
1293
    }
1294
    return true;
1295
}
1296

1297
#if AP_BATTERY_SCRIPTING_ENABLED
1298
/*
1299
  handle state update from a lua script
1300
 */
1301
bool AP_BattMonitor::handle_scripting(uint8_t idx, const BattMonitorScript_State &_state)
1302
{
1303
    if (idx >= _num_instances) {
1304
        return false;
1305
    }
1306
    return drivers[idx]->handle_scripting(_state);
1307
}
1308
#endif
1309

1310
namespace AP {
1311

1312
AP_BattMonitor &battery()
1313
{
1314
    return *AP_BattMonitor::get_singleton();
1315
}
1316

1317
};
1318

1319
#endif  // AP_BATTERY_ENABLED
1320

1321
Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

Product

Resources

Company

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more, all in one place. Commercial Alternative to JupyterHub.

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
