CoCalc -- AP_BattMonitor.cpp

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
#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
    AP_SUBGROUPVARPTR(drivers[0], "_", 41, AP_BattMonitor, backend_var_info[0]),
80

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

535
    AP_GROUPEND
536
};
537

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

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

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

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

566
    _highest_failsafe_priority = INT8_MAX;
567

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

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

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

728
        // param count could have changed
729
        AP_Param::invalidate_count();
730
    }
731

732
        // call init function for each backend
733
        if (drivers[instance] != nullptr) {
734
            drivers[instance]->init();
735
            // _num_instances is actually the index for looping over instances
736
            // the user may have BATT_MONITOR=0 and BATT2_MONITOR=7, in which case
737
            // there will be a gap, but as we always check for drivers[instances] being nullptr
738
            // this is safe
739
            _num_instances = instance + 1;
740
        }
741
    }
742
}
743

744
// read - For all active instances read voltage & current; log BAT, BCL, POWR, MCU
745
void AP_BattMonitor::read()
746
{
747
#if HAL_LOGGING_ENABLED
748
    AP_Logger *logger = AP_Logger::get_singleton();
749
    if (logger != nullptr && logger->should_log(_log_battery_bit)) {
750
        logger->Write_Power();
751
    }
752
#endif
753

754
    const uint32_t now_ms = AP_HAL::millis();
755
    for (uint8_t i=0; i<_num_instances; i++) {
756
            if (drivers[i] == nullptr) {
757
                continue;
758
            }
759
            if (allocated_type(i) != configured_type(i)) {
760
                continue;
761
            }
762
            // allow run-time disabling; this is technically redundant
763
            if (configured_type(i) == Type::NONE) {
764
                continue;
765
            }
766
            drivers[i]->read();
767
            drivers[i]->update_resistance_estimate();
768

769
#if AP_BATTERY_ESC_TELEM_OUTBOUND_ENABLED
770
            drivers[i]->update_esc_telem_outbound();
771
#endif
772

773
            // Update last heathy timestamp
774
            if (state[i].healthy) {
775
                state[i].last_healthy_ms = now_ms;
776
            }
777

778
#if HAL_LOGGING_ENABLED
779
            if (logger != nullptr && logger->should_log(_log_battery_bit)) {
780
                const uint64_t time_us = AP_HAL::micros64();
781
                drivers[i]->Log_Write_BAT(i, time_us);
782
                drivers[i]->Log_Write_BCL(i, time_us);
783
            }
784
#endif
785
    }
786

787
    check_failsafes();
788
    
789
    checkPoweringOff();
790
}
791

792
// healthy - returns true if monitor is functioning
793
bool AP_BattMonitor::healthy(uint8_t instance) const {
794
    return instance < _num_instances && state[instance].healthy;
795
}
796

797
/// voltage - returns battery voltage in volts
798
float AP_BattMonitor::voltage(uint8_t instance) const
799
{
800
    if (instance < _num_instances) {
801
        return state[instance].voltage;
802
    } else {
803
        return 0.0f;
804
    }
805
}
806

807
/// get voltage with sag removed (based on battery current draw and resistance)
808
/// this will always be greater than or equal to the raw voltage
809
float AP_BattMonitor::voltage_resting_estimate(uint8_t instance) const
810
{
811
    if (instance < _num_instances && drivers[instance] != nullptr) {
812
        return drivers[instance]->voltage_resting_estimate();
813
    } else {
814
        return 0.0f;
815
    }
816
}
817

818
/// voltage - returns battery voltage in volts for GCS, may be resting voltage if option enabled
819
float AP_BattMonitor::gcs_voltage(uint8_t instance) const
820
{
821
    if (instance >= _num_instances || drivers[instance] == nullptr) {
822
        return 0.0f;
823
    }
824
    if (drivers[instance]->option_is_set(AP_BattMonitor_Params::Options::GCS_Resting_Voltage)) {
825
        return voltage_resting_estimate(instance);
826
    }
827
    return state[instance].voltage;
828
}
829

830
bool AP_BattMonitor::option_is_set(uint8_t instance, AP_BattMonitor_Params::Options option) const
831
{
832
    if (instance >= _num_instances || drivers[instance] == nullptr) {
833
        return false;
834
    }
835
    return drivers[instance]->option_is_set(option);
836
}
837

838
/// current_amps - returns the instantaneous current draw in amperes
839
bool AP_BattMonitor::current_amps(float &current, uint8_t instance) const {
840
    if ((instance < _num_instances) && (drivers[instance] != nullptr) && drivers[instance]->has_current()) {
841
        current = state[instance].current_amps;
842
        return true;
843
    } else {
844
        return false;
845
    }
846
}
847

848
/// consumed_mah - returns total current drawn since start-up in milliampere.hours
849
bool AP_BattMonitor::consumed_mah(float &mah, const uint8_t instance) const {
850
    if ((instance < _num_instances) && (drivers[instance] != nullptr) && drivers[instance]->has_current()) {
851
        const float consumed_mah = state[instance].consumed_mah;
852
        if (isnan(consumed_mah)) {
853
            return false;
854
        }
855
        mah = consumed_mah;
856
        return true;
857
    } else {
858
        return false;
859
    }
860
}
861

862
/// consumed_wh - returns energy consumed since start-up in Watt.hours
863
bool AP_BattMonitor::consumed_wh(float &wh, const uint8_t instance) const {
864
    if (instance < _num_instances && drivers[instance] != nullptr && drivers[instance]->has_consumed_energy()) {
865
        wh = state[instance].consumed_wh;
866
        return true;
867
    } else {
868
        return false;
869
    }
870
}
871

872
/// capacity_remaining_pct - returns true if the percentage is valid and writes to percentage argument
873
bool AP_BattMonitor::capacity_remaining_pct(uint8_t &percentage, uint8_t instance) const
874
{
875
    if (instance < _num_instances && drivers[instance] != nullptr) {
876
        return drivers[instance]->capacity_remaining_pct(percentage);
877
    }
878
    return false;
879
}
880

881
/// time_remaining - returns remaining battery time
882
bool AP_BattMonitor::time_remaining(uint32_t &seconds, uint8_t instance) const
883
{
884
    if (instance < _num_instances && drivers[instance] != nullptr && state[instance].has_time_remaining) {
885
        seconds = state[instance].time_remaining;
886
        return true;
887
    }
888
    return false;
889
}
890

891
/// pack_capacity_mah - returns the capacity of the battery pack in mAh when the pack is full
892
int32_t AP_BattMonitor::pack_capacity_mah(uint8_t instance) const
893
{
894
    if (instance < AP_BATT_MONITOR_MAX_INSTANCES) {
895
        return _params[instance]._pack_capacity;
896
    } else {
897
        return 0;
898
    }
899
}
900

901
void AP_BattMonitor::check_failsafes(void)
902
{
903
    if (hal.util->get_soft_armed()) {
904
        for (uint8_t i = 0; i < _num_instances; i++) {
905
            if (drivers[i] == nullptr) {
906
                continue;
907
            }
908

909
            const Failsafe type = drivers[i]->update_failsafes();
910
            if (type <= state[i].failsafe) {
911
                continue;
912
            }
913

914
            int8_t action = 0;
915
            const char *type_str = nullptr;
916
            switch (type) {
917
                case Failsafe::None:
918
                    continue; // should not have been called in this case
919
                case Failsafe::Unhealthy:
920
                    // Report only for unhealthy, could add action param in the future
921
                    action = 0;
922
                    type_str = "missing, last:";
923
                    break;
924
                case Failsafe::Low:
925
                    action = _params[i]._failsafe_low_action;
926
                    type_str = "low";
927
                    break;
928
                case Failsafe::Critical:
929
                    action = _params[i]._failsafe_critical_action;
930
                    type_str = "critical";
931
                    break;
932
            }
933

934
            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Battery %d is %s %.2fV used %.0f mAh", i + 1, type_str,
935
                            (double)voltage(i), (double)state[i].consumed_mah);
936
            _has_triggered_failsafe = true;
937
#ifndef HAL_BUILD_AP_PERIPH
938
            AP_Notify::flags.failsafe_battery = true;
939
#endif
940
            state[i].failsafe = type;
941

942
            // map the desired failsafe action to a priority level
943
            int8_t priority = 0;
944
            if (_failsafe_priorities != nullptr) {
945
                while (_failsafe_priorities[priority] != -1) {
946
                    if (_failsafe_priorities[priority] == action) {
947
                        break;
948
                    }
949
                    priority++;
950
                }
951

952
            }
953

954
            // trigger failsafe if the action was equal or higher priority
955
            // It's valid to retrigger the same action if a different battery provoked the event
956
            if (priority <= _highest_failsafe_priority) {
957
                _battery_failsafe_handler_fn(type_str, action);
958
                _highest_failsafe_priority = priority;
959
            }
960
        }
961
    }
962
}
963

964
// return true if any battery is pushing too much power
965
bool AP_BattMonitor::overpower_detected() const
966
{
967
#if AP_BATTERY_WATT_MAX_ENABLED && APM_BUILD_TYPE(APM_BUILD_ArduPlane)
968
    for (uint8_t instance = 0; instance < _num_instances; instance++) {
969
        if (overpower_detected(instance)) {
970
            return true;
971
        }
972
    }
973
#endif
974
    return false;
975
}
976

977
bool AP_BattMonitor::overpower_detected(uint8_t instance) const
978
{
979
#if AP_BATTERY_WATT_MAX_ENABLED && APM_BUILD_TYPE(APM_BUILD_ArduPlane)
980
    if (instance < _num_instances && _params[instance]._watt_max > 0) {
981
        const float power = state[instance].current_amps * state[instance].voltage;
982
        return state[instance].healthy && (power > _params[instance]._watt_max);
983
    }
984
#endif
985
    return false;
986
}
987

988
bool AP_BattMonitor::has_cell_voltages(const uint8_t instance) const
989
{
990
    if (instance < _num_instances && drivers[instance] != nullptr) {
991
        return drivers[instance]->has_cell_voltages();
992
    }
993

994
    return false;
995
}
996

997
// return the current cell voltages, returns the first monitor instances cells if the instance is out of range
998
const AP_BattMonitor::cells & AP_BattMonitor::get_cell_voltages(const uint8_t instance) const
999
{
1000
    if (instance >= AP_BATT_MONITOR_MAX_INSTANCES) {
1001
        return state[AP_BATT_PRIMARY_INSTANCE].cell_voltages;
1002
    } else {
1003
        return state[instance].cell_voltages;
1004
    }
1005
}
1006

1007
// get once cell voltage (for scripting)
1008
bool AP_BattMonitor::get_cell_voltage(uint8_t instance, uint8_t cell, float &voltage) const
1009
{
1010
    if (!has_cell_voltages(instance) ||
1011
        cell >= AP_BATT_MONITOR_CELLS_MAX) {
1012
        return false;
1013
    }
1014
    const auto &cell_voltages = get_cell_voltages(instance);
1015
    const uint16_t voltage_mv = cell_voltages.cells[cell];
1016
    if (voltage_mv == 0 || voltage_mv == UINT16_MAX) {
1017
        // UINT16_MAX is used as invalid indicator
1018
        return false;
1019
    }
1020
    voltage = voltage_mv*0.001;
1021
    return true;
1022
}
1023

1024
// returns true if there is a temperature reading
1025
bool AP_BattMonitor::get_temperature(float &temperature, const uint8_t instance) const
1026
{
1027
    if (instance >= _num_instances || drivers[instance] == nullptr) {
1028
        return false;
1029
    }
1030

1031
    return drivers[instance]->get_temperature(temperature);
1032
}
1033

1034
#if AP_TEMPERATURE_SENSOR_ENABLED
1035
// return true when successfully setting a battery temperature from an external source by instance
1036
bool AP_BattMonitor::set_temperature(const float temperature, const uint8_t instance)
1037
{
1038
    if (instance >= _num_instances || drivers[instance] == nullptr) {
1039
        return false;
1040
    }
1041
    state[instance].temperature_external = temperature;
1042
    state[instance].temperature_external_use = true;
1043
    return true;
1044
}
1045

1046
// return true when successfully setting a battery temperature from an external source by serial_number
1047
bool AP_BattMonitor::set_temperature_by_serial_number(const float temperature, const int32_t serial_number)
1048
{
1049
    bool success = false;
1050
    for (uint8_t i = 0; i < _num_instances; i++) {
1051
        if (drivers[i] != nullptr && get_serial_number(i) == serial_number) {
1052
            success |= set_temperature(temperature, i);
1053
        }
1054
    }
1055
    return success;
1056
}
1057
#endif // AP_TEMPERATURE_SENSOR_ENABLED
1058

1059
// return true if cycle count can be provided and fills in cycles argument
1060
bool AP_BattMonitor::get_cycle_count(uint8_t instance, uint16_t &cycles) const
1061
{
1062
    if (instance >= _num_instances || (drivers[instance] == nullptr)) {
1063
        return false;
1064
    }
1065
    return drivers[instance]->get_cycle_count(cycles);
1066
}
1067

1068
bool AP_BattMonitor::arming_checks(size_t buflen, char *buffer) const
1069
{
1070
    char temp_buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1] {};
1071

1072
    for (uint8_t i = 0; i < AP_BATT_MONITOR_MAX_INSTANCES; i++) {
1073
        const auto expected_type = configured_type(i);
1074

1075
        if (drivers[i] == nullptr && expected_type == Type::NONE) {
1076
            continue;
1077
        }
1078

1079
#if !AP_BATTERY_SUM_ENABLED
1080
        // CONVERSION - Added Sep 2024 for ArduPilot 4.6 as we are
1081
        // removing the SUM backend on 1MB boards.  Give a
1082
        // more-specific error for the sum backend:
1083
        if (expected_type == Type::Sum) {
1084
            hal.util->snprintf(buffer, buflen, "Battery %d %s", i + 1, "feature BATTERY_SUM not available");
1085
            return false;
1086
        }
1087
#endif
1088

1089
        if (drivers[i] == nullptr || allocated_type(i) != expected_type) {
1090
            hal.util->snprintf(buffer, buflen, "Battery %d %s", i + 1, "unhealthy");
1091
            return false;
1092
        }
1093

1094
        if (!drivers[i]->arming_checks(temp_buffer, sizeof(temp_buffer))) {
1095
            hal.util->snprintf(buffer, buflen, "Battery %d %s", i + 1, temp_buffer);
1096
            return false;
1097
        }
1098
    }
1099

1100
    return true;
1101
}
1102

1103
// Check's each smart battery instance for its powering off state and broadcasts notifications
1104
void AP_BattMonitor::checkPoweringOff(void)
1105
{
1106
    for (uint8_t i = 0; i < _num_instances; i++) {
1107
        if (state[i].is_powering_off && !state[i].powerOffNotified) {
1108
#ifndef HAL_BUILD_AP_PERIPH
1109
            // Set the AP_Notify flag, which plays the power off tones
1110
            AP_Notify::flags.powering_off = true;
1111
#endif
1112

1113
            // Send a Mavlink broadcast announcing the shutdown
1114
#if HAL_GCS_ENABLED
1115
            mavlink_command_long_t cmd_msg{};
1116
            cmd_msg.command = MAV_CMD_POWER_OFF_INITIATED;
1117
            cmd_msg.param1 = i+1;
1118
            GCS_MAVLINK::send_to_components(MAVLINK_MSG_ID_COMMAND_LONG, (char*)&cmd_msg, sizeof(cmd_msg));
1119
            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Vehicle %d battery %d is powering off", mavlink_system.sysid, i+1);
1120
#endif
1121

1122
            // only send this once
1123
            state[i].powerOffNotified = true;
1124
        }
1125
    }
1126
}
1127

1128
/*
1129
  reset battery remaining percentage for batteries that integrate to
1130
  calculate percentage remaining
1131
*/
1132
bool AP_BattMonitor::reset_remaining_mask(uint16_t battery_mask, float percentage)
1133
{
1134
    static_assert(AP_BATT_MONITOR_MAX_INSTANCES <= 16, "More batteries are enabled then can be reset");
1135
    bool ret = true;
1136
    Failsafe highest_failsafe = Failsafe::None;
1137
    for (uint8_t i = 0; i < _num_instances; i++) {
1138
        if ((1U<<i) & battery_mask) {
1139
            if (drivers[i] != nullptr) {
1140
                ret &= drivers[i]->reset_remaining(percentage);
1141
            } else {
1142
                ret = false;
1143
            }
1144
        }
1145
        if (state[i].failsafe > highest_failsafe) {
1146
            highest_failsafe = state[i].failsafe;
1147
        }
1148
    }
1149

1150
    // If all backends are not in failsafe then set overall failsafe state
1151
    if (highest_failsafe == Failsafe::None) {
1152
        _highest_failsafe_priority = INT8_MAX;
1153
        _has_triggered_failsafe = false;
1154
        // and reset notify flag
1155
        AP_Notify::flags.failsafe_battery = false;
1156
    }
1157
    return ret;
1158
}
1159

1160
// Returns the mavlink charge state. The following mavlink charge states are not used
1161
// MAV_BATTERY_CHARGE_STATE_EMERGENCY , MAV_BATTERY_CHARGE_STATE_FAILED
1162
// MAV_BATTERY_CHARGE_STATE_UNHEALTHY, MAV_BATTERY_CHARGE_STATE_CHARGING
1163
MAV_BATTERY_CHARGE_STATE AP_BattMonitor::get_mavlink_charge_state(const uint8_t instance) const 
1164
{
1165
    if (instance >= _num_instances) {
1166
        return MAV_BATTERY_CHARGE_STATE_UNDEFINED;
1167
    }
1168

1169
    switch (state[instance].failsafe) {
1170

1171
    case Failsafe::None:
1172
    case Failsafe::Unhealthy:
1173
        if (get_mavlink_fault_bitmask(instance) != 0 || !healthy()) {
1174
            return MAV_BATTERY_CHARGE_STATE_UNHEALTHY;
1175
        }
1176
        return MAV_BATTERY_CHARGE_STATE_OK;
1177

1178
    case Failsafe::Low:
1179
        return MAV_BATTERY_CHARGE_STATE_LOW;
1180

1181
    case Failsafe::Critical:
1182
        return MAV_BATTERY_CHARGE_STATE_CRITICAL;
1183
    }
1184

1185
    // Should not reach this
1186
    return MAV_BATTERY_CHARGE_STATE_UNDEFINED;
1187
}
1188

1189
// Returns mavlink fault state
1190
uint32_t AP_BattMonitor::get_mavlink_fault_bitmask(const uint8_t instance) const
1191
{
1192
    if (instance >= _num_instances || drivers[instance] == nullptr) {
1193
        return 0;
1194
    }
1195
    return drivers[instance]->get_mavlink_fault_bitmask();
1196
}
1197

1198
// return true if state of health (as a percentage) can be provided and fills in soh_pct argument
1199
bool AP_BattMonitor::get_state_of_health_pct(uint8_t instance, uint8_t &soh_pct) const
1200
{
1201
    if (instance >= _num_instances || drivers[instance] == nullptr) {
1202
        return false;
1203
    }
1204
    return drivers[instance]->get_state_of_health_pct(soh_pct);
1205
}
1206

1207
// Enable/Disable (Turn on/off) power to all backends who are MPPTs or BMSs
1208
void AP_BattMonitor::set_powered_state_to_all(const bool power_on)
1209
{
1210
    for (uint8_t i=0; i < _num_instances; i++) {
1211
        set_powered_state(i, power_on);
1212
    }
1213
}
1214

1215
// Enable/Disable (Turn on/off) power. When disabled, the MPPT or BMS does not
1216
// supply energy to the system regardless if it's capable to or not. When enabled
1217
// it will supply energy if available.
1218
void AP_BattMonitor::set_powered_state(const uint8_t instance, const bool power_on)
1219
{
1220
    if (instance < _num_instances && drivers[instance] != nullptr) {
1221
        drivers[instance]->set_powered_state(power_on);
1222
    }
1223
}
1224

1225
/*
1226
  check that all configured battery monitors are healthy
1227
 */
1228
bool AP_BattMonitor::healthy() const
1229
{
1230
    for (uint8_t i=0; i< _num_instances; i++) {
1231
        if (allocated_type(i) != configured_type(i)) {
1232
            return false;
1233
        }
1234
        // allow run-time disabling; this is technically redundant
1235
        if (configured_type(i) == Type::NONE) {
1236
            continue;
1237
        }
1238
        if (!healthy(i)) {
1239
            return false;
1240
        }
1241
    }
1242
    return true;
1243
}
1244

1245
#if AP_BATTERY_SCRIPTING_ENABLED
1246
/*
1247
  handle state update from a lua script
1248
 */
1249
bool AP_BattMonitor::handle_scripting(uint8_t idx, const BattMonitorScript_State &_state)
1250
{
1251
    if (idx >= _num_instances) {
1252
        return false;
1253
    }
1254
    if (drivers[idx] == nullptr) {
1255
        return false;
1256
    }
1257
    return drivers[idx]->handle_scripting(_state);
1258
}
1259
#endif
1260

1261
namespace AP {
1262

1263
AP_BattMonitor &battery()
1264
{
1265
    return *AP_BattMonitor::get_singleton();
1266
}
1267

1268
};
1269

1270
#endif  // AP_BATTERY_ENABLED
1271

1272
Product

Resources

Company
