Line data Source code
1 : #include "AP_Logger.h"
2 :
3 : #if HAL_LOGGING_ENABLED
4 :
5 : #include "AP_Logger_Backend.h"
6 :
7 : #include "AP_Logger_File.h"
8 : #include "AP_Logger_Flash_JEDEC.h"
9 : #include "AP_Logger_W25NXX.h"
10 : #include "AP_Logger_MAVLink.h"
11 :
12 : #include <AP_InternalError/AP_InternalError.h>
13 : #include <GCS_MAVLink/GCS.h>
14 : #include <AP_BoardConfig/AP_BoardConfig.h>
15 : #include <AP_Rally/AP_Rally.h>
16 : #include <AP_Vehicle/AP_Vehicle_Type.h>
17 :
18 : #if HAL_LOGGER_FENCE_ENABLED
19 : #include <AC_Fence/AC_Fence.h>
20 : #endif
21 :
22 : AP_Logger *AP_Logger::_singleton;
23 :
24 : extern const AP_HAL::HAL& hal;
25 :
26 : #ifndef HAL_LOGGING_FILE_BUFSIZE
27 : #if HAL_MEM_CLASS >= HAL_MEM_CLASS_1000
28 : // adjust buffer size for extra space allocated on more capable boards
29 : #define HAL_LOGGING_FILE_BUFSIZE (200-((AP_FATFS_MAX_IO_SIZE-AP_FATFS_MIN_IO_SIZE)/1024))
30 : #elif HAL_MEM_CLASS >= HAL_MEM_CLASS_500
31 : #define HAL_LOGGING_FILE_BUFSIZE 80
32 : #elif HAL_MEM_CLASS >= HAL_MEM_CLASS_300
33 : #define HAL_LOGGING_FILE_BUFSIZE 50
34 : #else
35 : #define HAL_LOGGING_FILE_BUFSIZE 16
36 : #endif
37 : #endif
38 :
39 : #ifndef HAL_LOGGING_DATAFLASH_DRIVER
40 : #define HAL_LOGGING_DATAFLASH_DRIVER AP_Logger_Flash_JEDEC
41 : #endif
42 :
43 : #ifndef HAL_LOGGING_STACK_SIZE
44 : #define HAL_LOGGING_STACK_SIZE 1580
45 : #endif
46 :
47 : #ifndef HAL_LOGGING_MAV_BUFSIZE
48 : #define HAL_LOGGING_MAV_BUFSIZE 8
49 : #endif
50 :
51 : #ifndef HAL_LOGGING_FILE_TIMEOUT
52 : #define HAL_LOGGING_FILE_TIMEOUT 5
53 : #endif
54 :
55 : // by default log for 15 seconds after disarming
56 : #ifndef HAL_LOGGER_ARM_PERSIST
57 : #define HAL_LOGGER_ARM_PERSIST 15
58 : #endif
59 :
60 : #ifndef HAL_LOGGER_MIN_MB_FREE
61 : #if AP_FILESYSTEM_LITTLEFS_ENABLED
62 : #if AP_FILESYSTEM_LITTLEFS_FLASH_TYPE == AP_FILESYSTEM_FLASH_W25NXX
63 : #define HAL_LOGGER_MIN_MB_FREE 10
64 : #else
65 : #define HAL_LOGGER_MIN_MB_FREE 2
66 : #endif
67 : #else
68 : #define HAL_LOGGER_MIN_MB_FREE 500
69 : #endif
70 : #endif
71 :
72 : #ifndef HAL_LOGGING_BACKENDS_DEFAULT
73 : # if HAL_LOGGING_FILESYSTEM_ENABLED && (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
74 : # define HAL_LOGGING_BACKENDS_DEFAULT Backend_Type::FILESYSTEM
75 : # elif HAL_LOGGING_DATAFLASH_ENABLED
76 : # define HAL_LOGGING_BACKENDS_DEFAULT Backend_Type::BLOCK
77 : # elif HAL_LOGGING_FILESYSTEM_ENABLED
78 : # define HAL_LOGGING_BACKENDS_DEFAULT Backend_Type::FILESYSTEM
79 : # elif HAL_LOGGING_MAVLINK_ENABLED
80 : # define HAL_LOGGING_BACKENDS_DEFAULT Backend_Type::MAVLINK
81 : # else
82 : # define HAL_LOGGING_BACKENDS_DEFAULT 0
83 : # endif
84 : #endif
85 :
86 : // when adding new msgs we start at a different index in replay
87 : #if APM_BUILD_TYPE(APM_BUILD_Replay)
88 : #define LOGGING_FIRST_DYNAMIC_MSGID REPLAY_LOG_NEW_MSG_MAX
89 : #else
90 : #define LOGGING_FIRST_DYNAMIC_MSGID 254
91 : #endif
92 :
93 : static constexpr uint16_t MAX_LOG_FILES = 500;
94 : static constexpr uint16_t MIN_LOG_FILES = 2;
95 :
96 : const AP_Param::GroupInfo AP_Logger::var_info[] = {
97 : // @Param: _BACKEND_TYPE
98 : // @DisplayName: AP_Logger Backend Storage type
99 : // @Description: Bitmap of what Logger backend types to enable. Block-based logging is available on SITL and boards with dataflash chips. Multiple backends can be selected.
100 : // @Bitmask: 0:File,1:MAVLink,2:Block
101 : // @User: Standard
102 : // @RebootRequired: True
103 : AP_GROUPINFO("_BACKEND_TYPE", 0, AP_Logger, _params.backend_types, uint8_t(HAL_LOGGING_BACKENDS_DEFAULT)),
104 :
105 : // @Param: _FILE_BUFSIZE
106 : // @DisplayName: Logging File and Block Backend buffer size max (in kibibytes)
107 : // @Description: The File and Block backends use a buffer to store data before writing to the block device. Raising this value may reduce "gaps" in your SD card logging but increases memory usage. This buffer size may be reduced to free up available memory
108 : // @Units: KiB
109 : // @Range: 4 200
110 : // @User: Standard
111 : AP_GROUPINFO("_FILE_BUFSIZE", 1, AP_Logger, _params.file_bufsize, HAL_LOGGING_FILE_BUFSIZE),
112 :
113 : // @Param: _DISARMED
114 : // @DisplayName: Enable logging while disarmed
115 : // @Description: If LOG_DISARMED is set to 1 then logging will be enabled at all times including when disarmed. Logging before arming can make for very large logfiles but can help a lot when tracking down startup issues and is necessary if logging of EKF replay data is selected via the LOG_REPLAY parameter. If LOG_DISARMED is set to 2, then logging will be enabled when disarmed, but not if a USB connection is detected. This can be used to prevent unwanted data logs being generated when the vehicle is connected via USB for log downloading or parameter changes. If LOG_DISARMED is set to 3 then logging will happen while disarmed, but if the vehicle never arms then the logs using the filesystem backend will be discarded on the next boot.
116 : // @Values: 0:Disabled,1:Enabled,2:Disabled on USB connection,3:Discard log on reboot if never armed
117 : // @User: Standard
118 : AP_GROUPINFO("_DISARMED", 2, AP_Logger, _params.log_disarmed, 0),
119 :
120 : // @Param: _REPLAY
121 : // @DisplayName: Enable logging of information needed for Replay
122 : // @Description: If LOG_REPLAY is set to 1 then the EKF2 and EKF3 state estimators will log detailed information needed for diagnosing problems with the Kalman filter. LOG_DISARMED must be set to 1 or 2 or else the log will not contain the pre-flight data required for replay testing of the EKF's. It is suggested that you also raise LOG_FILE_BUFSIZE to give more buffer space for logging and use a high quality microSD card to ensure no sensor data is lost.
123 : // @Values: 0:Disabled,1:Enabled
124 : // @User: Standard
125 : AP_GROUPINFO("_REPLAY", 3, AP_Logger, _params.log_replay, 0),
126 :
127 : // @Param: _FILE_DSRMROT
128 : // @DisplayName: Stop logging to current file on disarm
129 : // @Description: When set, the current log file is closed when the vehicle is disarmed. If LOG_DISARMED is set then a fresh log will be opened. Applies to the File and Block logging backends.
130 : // @Values: 0:Disabled,1:Enabled
131 : // @User: Standard
132 : AP_GROUPINFO("_FILE_DSRMROT", 4, AP_Logger, _params.file_disarm_rot, 0),
133 :
134 : #if HAL_LOGGING_MAVLINK_ENABLED
135 : // @Param: _MAV_BUFSIZE
136 : // @DisplayName: Maximum AP_Logger MAVLink Backend buffer size
137 : // @Description: Maximum amount of memory to allocate to AP_Logger-over-mavlink
138 : // @User: Advanced
139 : // @Units: kB
140 : AP_GROUPINFO("_MAV_BUFSIZE", 5, AP_Logger, _params.mav_bufsize, HAL_LOGGING_MAV_BUFSIZE),
141 : #endif
142 :
143 : // @Param: _FILE_TIMEOUT
144 : // @DisplayName: Timeout before giving up on file writes
145 : // @Description: This controls the amount of time before failing writes to a log file cause the file to be closed and logging stopped.
146 : // @User: Standard
147 : // @Units: s
148 : AP_GROUPINFO("_FILE_TIMEOUT", 6, AP_Logger, _params.file_timeout, HAL_LOGGING_FILE_TIMEOUT),
149 :
150 : // @Param: _FILE_MB_FREE
151 : // @DisplayName: Old logs on the SD card will be deleted to maintain this amount of free space
152 : // @Description: Set this such that the free space is larger than your largest typical flight log
153 : // @Units: MB
154 : // @Range: 2 1000
155 : // @User: Standard
156 : AP_GROUPINFO("_FILE_MB_FREE", 7, AP_Logger, _params.min_MB_free, HAL_LOGGER_MIN_MB_FREE),
157 :
158 : // @Param: _FILE_RATEMAX
159 : // @DisplayName: Maximum logging rate for file backend
160 : // @Description: This sets the maximum rate that streaming log messages will be logged to the file backend. A value of zero means that rate limiting is disabled.
161 : // @Units: Hz
162 : // @Range: 0 1000
163 : // @Increment: 0.1
164 : // @User: Standard
165 : AP_GROUPINFO("_FILE_RATEMAX", 8, AP_Logger, _params.file_ratemax, 0),
166 :
167 : #if HAL_LOGGING_MAVLINK_ENABLED
168 : // @Param: _MAV_RATEMAX
169 : // @DisplayName: Maximum logging rate for mavlink backend
170 : // @Description: This sets the maximum rate that streaming log messages will be logged to the mavlink backend. A value of zero means that rate limiting is disabled.
171 : // @Units: Hz
172 : // @Range: 0 1000
173 : // @Increment: 0.1
174 : // @User: Standard
175 : AP_GROUPINFO("_MAV_RATEMAX", 9, AP_Logger, _params.mav_ratemax, 10),
176 : #endif
177 :
178 : #if HAL_LOGGING_BLOCK_ENABLED
179 : // @Param: _BLK_RATEMAX
180 : // @DisplayName: Maximum logging rate for block backend
181 : // @Description: This sets the maximum rate that streaming log messages will be logged to the block backend. A value of zero means that rate limiting is disabled.
182 : // @Units: Hz
183 : // @Range: 0 1000
184 : // @Increment: 0.1
185 : // @User: Standard
186 : AP_GROUPINFO("_BLK_RATEMAX", 10, AP_Logger, _params.blk_ratemax, 0),
187 : #endif
188 :
189 : // @Param: _DARM_RATEMAX
190 : // @DisplayName: Maximum logging rate when disarmed
191 : // @Description: This sets the maximum rate that streaming log messages will be logged to any backend when disarmed. A value of zero means that the normal backend rate limit is applied.
192 : // @Units: Hz
193 : // @Range: 0 1000
194 : // @Increment: 0.1
195 : // @User: Standard
196 : AP_GROUPINFO("_DARM_RATEMAX", 11, AP_Logger, _params.disarm_ratemax, 0),
197 :
198 : // @Param: _MAX_FILES
199 : // @DisplayName: Maximum number of log files
200 : // @Description: This sets the maximum number of log file that will be written on dataflash or sd card before starting to rotate log number. Limit is capped at 500 logs.
201 : // @Range: 2 500
202 : // @Increment: 1
203 : // @User: Advanced
204 : // @RebootRequired: True
205 : AP_GROUPINFO("_MAX_FILES", 12, AP_Logger, _params.max_log_files, MAX_LOG_FILES),
206 :
207 : AP_GROUPEND
208 : };
209 :
210 : #define streq(x, y) (!strcmp(x, y))
211 :
212 2100 : AP_Logger::AP_Logger()
213 : {
214 2100 : AP_Param::setup_object_defaults(this, var_info);
215 2100 : if (_singleton != nullptr) {
216 0 : AP_HAL::panic("AP_Logger must be singleton");
217 : }
218 :
219 2100 : _singleton = this;
220 2100 : }
221 :
222 2094 : void AP_Logger::init(const AP_Int32 &log_bitmask, const struct LogStructure *structures, uint8_t num_types)
223 : {
224 2094 : _log_bitmask = &log_bitmask;
225 :
226 2094 : if (hal.util->was_watchdog_armed()) {
227 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Forcing logging for watchdog reset");
228 0 : _params.log_disarmed.set(LogDisarmed::LOG_WHILE_DISARMED);
229 : }
230 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
231 2094 : validate_structures(structures, num_types);
232 2094 : dump_structures(structures, num_types);
233 : #endif
234 2094 : _num_types = num_types;
235 2094 : _structures = structures;
236 :
237 : // the "main" logging type needs to come before mavlink so that
238 : // index 0 is correct
239 : static const struct {
240 : Backend_Type type;
241 : AP_Logger_Backend* (*probe_fn)(AP_Logger&, LoggerMessageWriter_DFLogStart*);
242 : } backend_configs[] {
243 : #if HAL_LOGGING_FILESYSTEM_ENABLED
244 : { Backend_Type::FILESYSTEM, AP_Logger_File::probe },
245 : #endif
246 : #if HAL_LOGGING_DATAFLASH_ENABLED
247 : { Backend_Type::BLOCK, HAL_LOGGING_DATAFLASH_DRIVER::probe },
248 : #endif
249 : #if HAL_LOGGING_MAVLINK_ENABLED
250 : { Backend_Type::MAVLINK, AP_Logger_MAVLink::probe },
251 : #endif
252 : };
253 :
254 2094 : uint8_t remaining_types = _params.backend_types;
255 8376 : for (const auto &backend_config : backend_configs) {
256 6282 : uint8_t type = uint8_t(backend_config.type);
257 6282 : if ((remaining_types & type) == 0) {
258 4188 : continue; // skip if not enabled
259 : }
260 2094 : remaining_types &= ~type; // remember that we processed this type
261 2094 : if (_next_backend == LOGGER_MAX_BACKENDS) {
262 0 : AP_BoardConfig::config_error("Too many logger backends");
263 : return;
264 : }
265 :
266 : LoggerMessageWriter_DFLogStart *message_writer =
267 2094 : NEW_NOTHROW LoggerMessageWriter_DFLogStart();
268 2094 : if (message_writer == nullptr) {
269 0 : AP_BoardConfig::allocation_error("message writer");
270 : }
271 2094 : backends[_next_backend] = backend_config.probe_fn(*this, message_writer);
272 2094 : if (backends[_next_backend] == nullptr) {
273 0 : AP_BoardConfig::allocation_error("logger backend");
274 : }
275 2094 : _next_backend++;
276 : }
277 :
278 2094 : if (remaining_types) { // there was a type we didn't process
279 0 : AP_BoardConfig::config_error("Unknown logger backend");
280 : return;
281 : }
282 :
283 4188 : for (uint8_t i=0; i<_next_backend; i++) {
284 2094 : backends[i]->Init();
285 : }
286 :
287 2094 : start_io_thread();
288 :
289 2094 : EnableWrites(true);
290 : }
291 :
292 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
293 : #include <stdio.h>
294 :
295 : #define DEBUG_LOG_STRUCTURES 0
296 :
297 : extern const AP_HAL::HAL& hal;
298 : #define Debug(fmt, args ...) do {::fprintf(stderr, "%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
299 :
300 : /// return the number of commas present in string
301 407089 : static uint8_t count_commas(const char *string)
302 : {
303 407089 : uint8_t ret = 0;
304 16903968 : for (uint8_t i=0; i<strlen(string); i++) {
305 16496879 : if (string[i] == ',') {
306 3200052 : ret++;
307 : }
308 : }
309 407089 : return ret;
310 : }
311 :
312 : /// return a unit name given its ID
313 0 : const char* AP_Logger::unit_name(const uint8_t unit_id)
314 : {
315 0 : for (uint8_t i=0; i<_num_units; i++) {
316 0 : if (_units[i].ID == unit_id) {
317 0 : return _units[i].unit;
318 : }
319 : }
320 0 : return nullptr;
321 : }
322 :
323 : /// return a multiplier value given its ID
324 0 : double AP_Logger::multiplier_name(const uint8_t multiplier_id)
325 : {
326 0 : for (uint8_t i=0; i<_num_multipliers; i++) {
327 0 : if (_multipliers[i].ID == multiplier_id) {
328 0 : return _multipliers[i].multiplier;
329 : }
330 : }
331 : // Should we abort here?
332 0 : return 1.0f;
333 : }
334 :
335 : /// pretty-print field information from a log structure
336 0 : void AP_Logger::dump_structure_field(const struct LogStructure *logstructure, const char *label, const uint8_t fieldnum)
337 : {
338 0 : ::fprintf(stderr, " %s (%s)*(%f)\n", label, unit_name(logstructure->units[fieldnum]), multiplier_name(logstructure->multipliers[fieldnum]));
339 0 : }
340 :
341 : /// pretty-print log structures
342 : /// @note structures MUST be well-formed
343 2094 : void AP_Logger::dump_structures(const struct LogStructure *logstructures, const uint8_t num_types)
344 : {
345 : #if DEBUG_LOG_STRUCTURES
346 : for (uint16_t i=0; i<num_types; i++) {
347 : const struct LogStructure *logstructure = &logstructures[i];
348 : ::fprintf(stderr, "%s\n", logstructure->name);
349 : char label[32] = { };
350 : uint8_t labeloffset = 0;
351 : int8_t fieldnum = 0;
352 : for (uint8_t j=0; j<strlen(logstructure->labels); j++) {
353 : char labelchar = logstructure->labels[j];
354 : if (labelchar == '\0') {
355 : break;
356 : }
357 : if (labelchar == ',') {
358 : dump_structure_field(logstructure, label, fieldnum);
359 : fieldnum++;
360 : labeloffset = 0;
361 : memset(label, '\0', 32);
362 : } else {
363 : label[labeloffset++] = labelchar;
364 : }
365 : }
366 : dump_structure_field(logstructure, label, fieldnum);
367 : ::fprintf(stderr, "\n"); // just add a CR to the output
368 : }
369 : #endif
370 2094 : }
371 :
372 407089 : bool AP_Logger::labels_string_is_good(const char *labels) const
373 : {
374 407089 : bool passed = true;
375 407089 : if (strlen(labels) >= LS_LABELS_SIZE) {
376 0 : Debug("Labels string too long (%u >= %u)", unsigned(strlen(labels)), unsigned(LS_LABELS_SIZE));
377 0 : passed = false;
378 : }
379 : // This goes through and slices labels up into substrings by
380 : // changing commas to nulls - keeping references to each string in
381 : // label_offsets.
382 : char *label_offsets[LS_LABELS_SIZE];
383 407089 : uint8_t label_offsets_offset = 0;
384 407089 : char labels_copy[LS_LABELS_SIZE+1] {};
385 407089 : strncpy(labels_copy, labels, LS_LABELS_SIZE);
386 407089 : if (labels_copy[0] == ',') {
387 0 : Debug("Leading comma in (%s)", labels);
388 0 : passed = false;
389 : }
390 407089 : label_offsets[label_offsets_offset++] = labels_copy;
391 407089 : const uint8_t len = strnlen(labels_copy, LS_LABELS_SIZE);
392 16903968 : for (uint8_t i=0; i<len; i++) {
393 16496879 : if (labels_copy[i] == ',') {
394 3200052 : if (labels_copy[i+1] == '\0') {
395 0 : Debug("Trailing comma in (%s)", labels);
396 0 : passed = false;
397 0 : continue;
398 : }
399 3200052 : labels_copy[i] = '\0';
400 3200052 : label_offsets[label_offsets_offset++] = &labels_copy[i+1];
401 : }
402 : }
403 3607141 : for (uint8_t i=0; i<label_offsets_offset-1; i++) {
404 21058476 : for (uint8_t j=i+1; j<label_offsets_offset; j++) {
405 17858424 : if (!strcmp(label_offsets[i], label_offsets[j])) {
406 0 : Debug("Duplicate label (%s) in (%s)", label_offsets[i], labels);
407 0 : passed = false;
408 : }
409 : }
410 : }
411 407089 : return passed;
412 : }
413 :
414 407089 : bool AP_Logger::validate_structure(const struct LogStructure *logstructure, const int16_t offset)
415 : {
416 407089 : bool passed = true;
417 :
418 : #if DEBUG_LOG_STRUCTURES
419 : Debug("offset=%d ID=%d NAME=%s", offset, logstructure->msg_type, logstructure->name);
420 : #endif
421 :
422 : // fields must be null-terminated
423 : #define CHECK_ENTRY(fieldname,fieldname_s,fieldlen) \
424 : do { \
425 : if (strnlen(logstructure->fieldname, fieldlen) > fieldlen-1) { \
426 : Debug(" Message %s." fieldname_s " not NULL-terminated or too long", logstructure->name); \
427 : passed = false; \
428 : } \
429 : } while (false)
430 407089 : CHECK_ENTRY(name, "name", LS_NAME_SIZE);
431 407089 : CHECK_ENTRY(format, "format", LS_FORMAT_SIZE);
432 407089 : CHECK_ENTRY(labels, "labels", LS_LABELS_SIZE);
433 407089 : CHECK_ENTRY(units, "units", LS_UNITS_SIZE);
434 407089 : CHECK_ENTRY(multipliers, "multipliers", LS_MULTIPLIERS_SIZE);
435 : #undef CHECK_ENTRY
436 :
437 : // ensure each message ID is only used once
438 407089 : if (seen_ids[logstructure->msg_type]) {
439 0 : Debug(" ID %d used twice (LogStructure offset=%d)", logstructure->msg_type, offset);
440 0 : passed = false;
441 : }
442 407089 : seen_ids[logstructure->msg_type] = true;
443 :
444 : // ensure we have enough labels to cover columns
445 407089 : uint8_t fieldcount = strlen(logstructure->format);
446 407089 : uint8_t labelcount = count_commas(logstructure->labels)+1;
447 407089 : if (fieldcount != labelcount) {
448 0 : Debug(" %s fieldcount=%u does not match labelcount=%u",
449 : logstructure->name, fieldcount, labelcount);
450 0 : passed = false;
451 : }
452 :
453 407089 : if (!labels_string_is_good(logstructure->labels)) {
454 0 : passed = false;
455 : }
456 :
457 : // check that the structure is of an appropriate length to take fields
458 407089 : const int16_t msg_len = Write_calc_msg_len(logstructure->format);
459 407089 : if (msg_len != logstructure->msg_len) {
460 0 : Debug(" Calculated message length for (%s) based on format field (%s) does not match structure size (%d != %u)", logstructure->name, logstructure->format, msg_len, logstructure->msg_len);
461 0 : passed = false;
462 : }
463 :
464 : // ensure we have units for each field:
465 407089 : if (strlen(logstructure->units) != fieldcount) {
466 0 : Debug(" %s fieldcount=%u does not match unitcount=%u",
467 : logstructure->name, (unsigned)fieldcount, (unsigned)strlen(logstructure->units));
468 0 : passed = false;
469 : }
470 :
471 : // ensure we have multipliers for each field
472 407089 : if (strlen(logstructure->multipliers) != fieldcount) {
473 0 : Debug(" %s fieldcount=%u does not match multipliercount=%u",
474 : logstructure->name, (unsigned)fieldcount, (unsigned)strlen(logstructure->multipliers));
475 0 : passed = false;
476 : }
477 :
478 : // ensure the FMTU messages reference valid units
479 4014230 : for (uint8_t j=0; j<strlen(logstructure->units); j++) {
480 3607141 : char logunit = logstructure->units[j];
481 : uint8_t k;
482 41828375 : for (k=0; k<_num_units; k++) {
483 41828375 : if (logunit == _units[k].ID) {
484 : // found this one
485 3607141 : break;
486 : }
487 : }
488 3607141 : if (k == _num_units) {
489 0 : Debug(" invalid unit=%c", logunit);
490 0 : passed = false;
491 : }
492 : }
493 :
494 : // ensure the FMTU messages reference valid multipliers
495 4014230 : for (uint8_t j=0; j<strlen(logstructure->multipliers); j++) {
496 3607141 : char logmultiplier = logstructure->multipliers[j];
497 : uint8_t k;
498 13895198 : for (k=0; k<_num_multipliers; k++) {
499 13895198 : if (logmultiplier == _multipliers[k].ID) {
500 : // found this one
501 3607141 : break;
502 : }
503 : }
504 3607141 : if (k == _num_multipliers) {
505 0 : Debug(" invalid multiplier=%c", logmultiplier);
506 0 : passed = false;
507 : }
508 : }
509 :
510 : // ensure any float has a multiplier of zero
511 : if (false && passed) {
512 : for (uint8_t j=0; j<strlen(logstructure->multipliers); j++) {
513 : const char fmt = logstructure->format[j];
514 : if (fmt != 'f' && fmt != 'd' && fmt != 'g') {
515 : continue;
516 : }
517 : const char logmultiplier = logstructure->multipliers[j];
518 : if (logmultiplier == '0' ||
519 : logmultiplier == '?' ||
520 : logmultiplier == '-') {
521 : continue;
522 : }
523 : Debug(" %s[%u] float with non-zero multiplier=%c",
524 : logstructure->name,
525 : j,
526 : logmultiplier);
527 : passed = false;
528 : }
529 : }
530 :
531 407089 : return passed;
532 : }
533 :
534 2094 : void AP_Logger::validate_structures(const struct LogStructure *logstructures, const uint8_t num_types)
535 : {
536 2094 : Debug("Validating structures");
537 2094 : bool passed = true;
538 :
539 399697 : for (uint16_t i=0; i<num_types; i++) {
540 397603 : const struct LogStructure *logstructure = &logstructures[i];
541 397603 : passed = validate_structure(logstructure, i) && passed;
542 : }
543 :
544 : // ensure units are unique:
545 81666 : for (uint16_t i=0; i<ARRAY_SIZE(log_Units); i++) {
546 79572 : const struct UnitStructure &a = log_Units[i];
547 1551654 : for (uint16_t j=i+1; j<ARRAY_SIZE(log_Units); j++) {
548 1472082 : const struct UnitStructure &b = log_Units[j];
549 1472082 : if (a.ID == b.ID) {
550 0 : Debug("duplicate unit id=%c (%s/%s)", a.ID, a.unit, b.unit);
551 0 : passed = false;
552 : }
553 1472082 : if (streq(a.unit, b.unit)) {
554 0 : Debug("duplicate unit=%s (%c/%c)", a.unit, a.ID, b.ID);
555 0 : passed = false;
556 : }
557 : }
558 : }
559 :
560 : // ensure multipliers are unique:
561 33504 : for (uint16_t i=0; i<ARRAY_SIZE(log_Multipliers); i++) {
562 31410 : const struct MultiplierStructure &a = log_Multipliers[i];
563 251280 : for (uint16_t j=i+1; j<ARRAY_SIZE(log_Multipliers); j++) {
564 219870 : const struct MultiplierStructure &b = log_Multipliers[j];
565 219870 : if (a.ID == b.ID) {
566 0 : Debug("duplicate multiplier id=%c (%f/%f)",
567 : a.ID, a.multiplier, b.multiplier);
568 0 : passed = false;
569 : }
570 219870 : if (is_equal(a.multiplier, b.multiplier)) {
571 2094 : if (a.ID == '?' && b.ID == '0') {
572 : // special case
573 2094 : continue;
574 : }
575 0 : Debug("duplicate multiplier=%f (%c/%c)",
576 : a.multiplier, a.ID, b.ID);
577 0 : passed = false;
578 : }
579 : }
580 : }
581 :
582 2094 : if (!passed) {
583 0 : AP_BoardConfig::config_error("See console: Log structures invalid");
584 : }
585 2094 : }
586 :
587 : #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL
588 :
589 6734022023 : const struct LogStructure *AP_Logger::structure(uint16_t num) const
590 : {
591 6734022023 : return &_structures[num];
592 : }
593 :
594 1255233 : bool AP_Logger::logging_present() const
595 : {
596 1255233 : return _next_backend != 0;
597 : }
598 416870097 : bool AP_Logger::logging_enabled() const
599 : {
600 416870097 : if (_next_backend == 0) {
601 0 : return false;
602 : }
603 435084540 : for (uint8_t i=0; i<_next_backend; i++) {
604 416870097 : if (backends[i]->logging_enabled()) {
605 398655653 : return true;
606 : }
607 : }
608 18214443 : return false;
609 : }
610 1254200 : bool AP_Logger::logging_failed() const
611 : {
612 1254200 : if (_next_backend < 1) {
613 : // we should not have been called!
614 0 : return true;
615 : }
616 2507424 : for (uint8_t i=0; i<_next_backend; i++) {
617 1254200 : if (backends[i]->logging_failed()) {
618 976 : return true;
619 : }
620 : }
621 1253224 : return false;
622 : }
623 :
624 2840 : void AP_Logger::Write_MessageF(const char *fmt, ...)
625 : {
626 2840 : char msg[65] {}; // sizeof(log_Message.msg) + null-termination
627 :
628 : va_list ap;
629 2840 : va_start(ap, fmt);
630 2840 : hal.util->vsnprintf(msg, sizeof(msg), fmt, ap);
631 2840 : va_end(ap);
632 :
633 2840 : Write_Message(msg);
634 2840 : }
635 :
636 2079 : void AP_Logger::backend_starting_new_log(const AP_Logger_Backend *backend)
637 : {
638 2079 : _log_start_count++;
639 2079 : }
640 :
641 758853915 : bool AP_Logger::should_log(const uint32_t mask) const
642 : {
643 758853915 : bool armed = vehicle_is_armed();
644 :
645 758853943 : if (!(mask & *_log_bitmask)) {
646 699393635 : return false;
647 : }
648 59460344 : if (!armed && !log_while_disarmed()) {
649 3060400 : return false;
650 : }
651 56399919 : if (in_log_download()) {
652 3048 : return false;
653 : }
654 56396852 : if (_next_backend == 0) {
655 0 : return false;
656 : }
657 56396852 : return true;
658 : }
659 :
660 : /*
661 : return true if in log download which should prevent logging
662 : */
663 323223574 : bool AP_Logger::in_log_download() const
664 : {
665 323223574 : if (uint8_t(_params.backend_types) & uint8_t(Backend_Type::BLOCK)) {
666 : // when we have a BLOCK backend then listing completely prevents logging
667 957253 : return transfer_activity != TransferActivity::IDLE;
668 : }
669 : // for other backends listing does not interfere with logging
670 322266297 : return transfer_activity == TransferActivity::SENDING;
671 : }
672 :
673 78297 : const struct UnitStructure *AP_Logger::unit(uint16_t num) const
674 : {
675 78297 : return &_units[num];
676 : }
677 :
678 30856 : const struct MultiplierStructure *AP_Logger::multiplier(uint16_t num) const
679 : {
680 30856 : return &log_Multipliers[num];
681 : }
682 :
683 : #define FOR_EACH_BACKEND(methodcall) \
684 : do { \
685 : for (uint8_t i=0; i<_next_backend; i++) { \
686 : backends[i]->methodcall; \
687 : } \
688 : } while (0)
689 :
690 1033 : void AP_Logger::PrepForArming()
691 : {
692 2066 : FOR_EACH_BACKEND(PrepForArming());
693 1033 : }
694 :
695 2086 : void AP_Logger::setVehicle_Startup_Writer(vehicle_startup_message_Writer writer)
696 : {
697 2086 : _vehicle_messages = writer;
698 2086 : }
699 :
700 : #if AP_RTC_LOGGING_ENABLED
701 : // Write RTC data
702 7107 : void AP_Logger::Write_RTC()
703 : {
704 14174 : FOR_EACH_BACKEND(Write_RTC());
705 7107 : }
706 : #endif // AP_RTC_LOGGING_ENABLED
707 :
708 8379852 : void AP_Logger::set_vehicle_armed(const bool armed_state)
709 : {
710 8379852 : if (armed_state == _armed) {
711 : // no change in status
712 8377558 : return;
713 : }
714 2294 : _armed = armed_state;
715 :
716 2294 : if (_armed) {
717 : // went from disarmed to armed
718 : #if HAL_LOGGER_FILE_CONTENTS_ENABLED
719 : // get a set of @SYS files logged:
720 1162 : file_content_prepare_for_arming = true;
721 : #endif
722 : } else {
723 : // went from armed to disarmed
724 2264 : FOR_EACH_BACKEND(vehicle_was_disarmed());
725 : }
726 : }
727 :
728 : #if APM_BUILD_TYPE(APM_BUILD_Replay)
729 : /*
730 : remember formats for replay. This allows WriteV() to work within
731 : replay
732 : */
733 : void AP_Logger::save_format_Replay(const void *pBuffer)
734 : {
735 : if (((uint8_t *)pBuffer)[2] == LOG_FORMAT_MSG) {
736 : struct log_Format *fmt = (struct log_Format *)pBuffer;
737 : struct log_write_fmt *f = NEW_NOTHROW log_write_fmt;
738 : f->msg_type = fmt->type;
739 : f->msg_len = fmt->length;
740 : f->name = strndup(fmt->name, sizeof(fmt->name));
741 : f->fmt = strndup(fmt->format, sizeof(fmt->format));
742 : f->labels = strndup(fmt->labels, sizeof(fmt->labels));
743 : f->next = log_write_fmts;
744 : log_write_fmts = f;
745 : }
746 : }
747 : #endif
748 :
749 :
750 : // start functions pass straight through to backend:
751 172617168 : void AP_Logger::WriteBlock(const void *pBuffer, uint16_t size) {
752 : #if APM_BUILD_TYPE(APM_BUILD_Replay)
753 : save_format_Replay(pBuffer);
754 : #endif
755 345234237 : FOR_EACH_BACKEND(WriteBlock(pBuffer, size));
756 172617174 : }
757 :
758 : // only the first backend write need succeed for us to be successful
759 766629 : bool AP_Logger::WriteBlock_first_succeed(const void *pBuffer, uint16_t size)
760 : {
761 766629 : if (_next_backend == 0) {
762 0 : return false;
763 : }
764 :
765 766629 : for (uint8_t i=1; i<_next_backend; i++) {
766 0 : backends[i]->WriteBlock(pBuffer, size);
767 : }
768 :
769 766629 : return backends[0]->WriteBlock(pBuffer, size);
770 : }
771 :
772 : // write a replay block. This differs from other as it returns false if a backend doesn't
773 : // have space for the msg
774 167710066 : bool AP_Logger::WriteReplayBlock(uint8_t msg_id, const void *pBuffer, uint16_t size) {
775 167710066 : bool ret = true;
776 167710066 : if (log_replay()) {
777 772397 : uint8_t buf[3+size];
778 772397 : buf[0] = HEAD_BYTE1;
779 772397 : buf[1] = HEAD_BYTE2;
780 772397 : buf[2] = msg_id;
781 772397 : memcpy(&buf[3], pBuffer, size);
782 1544794 : for (uint8_t i=0; i<_next_backend; i++) {
783 772397 : if (!backends[i]->WritePrioritisedBlock(buf, sizeof(buf), true)) {
784 0 : ret = false;
785 : }
786 : }
787 772397 : }
788 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
789 : // things will almost certainly go sour. However, if we are not
790 : // logging while disarmed then the EKF can be started and trying
791 : // to log things even 'though the backends might be saying "no".
792 167710066 : if (!ret && log_while_disarmed()) {
793 0 : AP_HAL::panic("Failed to log replay block");
794 : }
795 : #endif
796 167710066 : return ret;
797 : }
798 :
799 220561 : void AP_Logger::WriteCriticalBlock(const void *pBuffer, uint16_t size) {
800 441122 : FOR_EACH_BACKEND(WriteCriticalBlock(pBuffer, size));
801 220561 : }
802 :
803 0 : void AP_Logger::WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical) {
804 0 : FOR_EACH_BACKEND(WritePrioritisedBlock(pBuffer, size, is_critical));
805 0 : }
806 :
807 : // change me to "DoTimeConsumingPreparations"?
808 8 : void AP_Logger::EraseAll() {
809 16 : FOR_EACH_BACKEND(EraseAll());
810 8 : }
811 : // change me to "LoggingAvailable"?
812 132942 : bool AP_Logger::CardInserted(void) {
813 132942 : for (uint8_t i=0; i< _next_backend; i++) {
814 132942 : if (backends[i]->CardInserted()) {
815 132942 : return true;
816 : }
817 : }
818 0 : return false;
819 : }
820 :
821 0 : void AP_Logger::StopLogging()
822 : {
823 0 : FOR_EACH_BACKEND(stop_logging());
824 0 : }
825 :
826 0 : uint16_t AP_Logger::find_last_log() const {
827 0 : if (_next_backend == 0) {
828 0 : return 0;
829 : }
830 0 : return backends[0]->find_last_log();
831 : }
832 18216 : void AP_Logger::get_log_boundaries(uint16_t log_num, uint32_t & start_page, uint32_t & end_page) {
833 18216 : if (_next_backend == 0) {
834 0 : return;
835 : }
836 18216 : backends[0]->get_log_boundaries(log_num, start_page, end_page);
837 : }
838 21682 : void AP_Logger::get_log_info(uint16_t log_num, uint32_t &size, uint32_t &time_utc) {
839 21682 : if (_next_backend == 0) {
840 0 : return;
841 : }
842 21682 : backends[0]->get_log_info(log_num, size, time_utc);
843 : }
844 365472 : int16_t AP_Logger::get_log_data(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data) {
845 365472 : if (_next_backend == 0) {
846 0 : return 0;
847 : }
848 365472 : return backends[0]->get_log_data(log_num, page, offset, len, data);
849 : }
850 18255 : uint16_t AP_Logger::get_num_logs(void) {
851 18255 : if (_next_backend == 0) {
852 0 : return 0;
853 : }
854 18255 : return backends[0]->get_num_logs();
855 : }
856 :
857 1898417 : uint16_t AP_Logger::get_max_num_logs() {
858 1898417 : const auto max_logs = constrain_uint16(_params.max_log_files.get(), MIN_LOG_FILES, MAX_LOG_FILES);
859 1898417 : if (_params.max_log_files.get() != max_logs) {
860 1 : _params.max_log_files.set_and_save_ifchanged(static_cast<int16_t>(max_logs));
861 : }
862 1898417 : return static_cast<uint16_t>(_params.max_log_files.get());
863 : }
864 :
865 : /* we're started if any of the backends are started */
866 44302073 : bool AP_Logger::logging_started(void) const {
867 46253433 : for (uint8_t i=0; i< _next_backend; i++) {
868 44302072 : if (backends[i]->logging_started()) {
869 42350712 : return true;
870 : }
871 : }
872 1951361 : return false;
873 : }
874 :
875 23305 : void AP_Logger::handle_mavlink_msg(GCS_MAVLINK &link, const mavlink_message_t &msg)
876 : {
877 23305 : switch (msg.msgid) {
878 4814 : case MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS:
879 9628 : FOR_EACH_BACKEND(remote_log_block_status_msg(link, msg));
880 4814 : break;
881 18491 : case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
882 : FALLTHROUGH;
883 : case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
884 : FALLTHROUGH;
885 : case MAVLINK_MSG_ID_LOG_ERASE:
886 : FALLTHROUGH;
887 : case MAVLINK_MSG_ID_LOG_REQUEST_END:
888 18491 : handle_log_message(link, msg);
889 18491 : break;
890 : }
891 23305 : }
892 :
893 41684408 : void AP_Logger::periodic_tasks() {
894 : #ifndef HAL_BUILD_AP_PERIPH
895 41684408 : handle_log_send();
896 : #endif
897 83368816 : FOR_EACH_BACKEND(periodic_tasks());
898 41684408 : }
899 :
900 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
901 : // currently only AP_Logger_File support this:
902 0 : void AP_Logger::flush(void) {
903 0 : FOR_EACH_BACKEND(flush());
904 0 : }
905 : #endif
906 :
907 :
908 1287 : void AP_Logger::Write_EntireMission()
909 : {
910 2574 : FOR_EACH_BACKEND(Write_EntireMission());
911 1287 : }
912 :
913 267613 : void AP_Logger::Write_Message(const char *message)
914 : {
915 535124 : FOR_EACH_BACKEND(Write_Message(message));
916 267613 : }
917 1027468 : void AP_Logger::Write_MessageChunk(uint8_t id, const char *messagechunk, uint8_t chunk_seq)
918 : {
919 2052936 : FOR_EACH_BACKEND(Write_MessageChunk(id, messagechunk, chunk_seq));
920 1027468 : }
921 :
922 9720 : void AP_Logger::Write_Mode(uint8_t mode, const ModeReason reason)
923 : {
924 19440 : FOR_EACH_BACKEND(Write_Mode(mode, reason));
925 9720 : }
926 :
927 97833 : void AP_Logger::Write_Parameter(const char *name, float value)
928 : {
929 195366 : FOR_EACH_BACKEND(Write_Parameter(name, value, quiet_nanf()));
930 97833 : }
931 :
932 2026 : void AP_Logger::Write_Mission_Cmd(const AP_Mission &mission,
933 : const AP_Mission::Mission_Command &cmd,
934 : LogMessages id)
935 : {
936 4052 : FOR_EACH_BACKEND(Write_Mission_Cmd(mission, cmd, id));
937 2026 : }
938 :
939 : #if HAL_RALLY_ENABLED
940 8 : void AP_Logger::Write_RallyPoint(uint8_t total,
941 : uint8_t sequence,
942 : const RallyLocation &rally_point)
943 : {
944 16 : FOR_EACH_BACKEND(Write_RallyPoint(total, sequence, rally_point));
945 8 : }
946 :
947 886 : void AP_Logger::Write_Rally()
948 : {
949 1772 : FOR_EACH_BACKEND(Write_Rally());
950 886 : }
951 : #endif
952 :
953 : #if HAL_LOGGER_FENCE_ENABLED
954 1001 : void AP_Logger::Write_Fence()
955 : {
956 2002 : FOR_EACH_BACKEND(Write_Fence());
957 1001 : }
958 : #endif
959 :
960 0 : void AP_Logger::Write_NamedValueFloat(const char *name, float value)
961 : {
962 0 : Write(
963 : "NVF",
964 : "TimeUS,Name,Value",
965 : "s#-",
966 : "F--",
967 : "QNf",
968 : AP_HAL::micros(),
969 : name,
970 : value
971 : );
972 0 : }
973 :
974 : // output a FMT message for each backend if not already done so
975 50963 : void AP_Logger::Safe_Write_Emit_FMT(log_write_fmt *f)
976 : {
977 101926 : for (uint8_t i=0; i<_next_backend; i++) {
978 50963 : backends[i]->Safe_Write_Emit_FMT(f->msg_type);
979 : }
980 50963 : }
981 :
982 1116947 : uint32_t AP_Logger::num_dropped() const
983 : {
984 1116947 : if (_next_backend == 0) {
985 0 : return 0;
986 : }
987 1116947 : return backends[0]->num_dropped();
988 : }
989 :
990 :
991 : // end functions pass straight through to backend
992 :
993 : /* Write support */
994 126204 : void AP_Logger::Write(const char *name, const char *labels, const char *fmt, ...)
995 : {
996 : va_list arg_list;
997 :
998 126204 : va_start(arg_list, fmt);
999 126204 : WriteV(name, labels, nullptr, nullptr, fmt, arg_list);
1000 126204 : va_end(arg_list);
1001 126204 : }
1002 :
1003 79313 : void AP_Logger::Write(const char *name, const char *labels, const char *units, const char *mults, const char *fmt, ...)
1004 : {
1005 : va_list arg_list;
1006 :
1007 79313 : va_start(arg_list, fmt);
1008 79313 : WriteV(name, labels, units, mults, fmt, arg_list);
1009 79313 : va_end(arg_list);
1010 79313 : }
1011 :
1012 42021092 : void AP_Logger::WriteStreaming(const char *name, const char *labels, const char *fmt, ...)
1013 : {
1014 : va_list arg_list;
1015 :
1016 42021092 : va_start(arg_list, fmt);
1017 42021092 : WriteV(name, labels, nullptr, nullptr, fmt, arg_list, false, true);
1018 42021092 : va_end(arg_list);
1019 42021092 : }
1020 :
1021 43138676 : void AP_Logger::WriteStreaming(const char *name, const char *labels, const char *units, const char *mults, const char *fmt, ...)
1022 : {
1023 : va_list arg_list;
1024 :
1025 43138676 : va_start(arg_list, fmt);
1026 43138676 : WriteV(name, labels, units, mults, fmt, arg_list, false, true);
1027 43138608 : va_end(arg_list);
1028 43138608 : }
1029 :
1030 1 : void AP_Logger::WriteCritical(const char *name, const char *labels, const char *fmt, ...)
1031 : {
1032 : va_list arg_list;
1033 :
1034 1 : va_start(arg_list, fmt);
1035 1 : WriteV(name, labels, nullptr, nullptr, fmt, arg_list, true);
1036 1 : va_end(arg_list);
1037 1 : }
1038 :
1039 0 : void AP_Logger::WriteCritical(const char *name, const char *labels, const char *units, const char *mults, const char *fmt, ...)
1040 : {
1041 : va_list arg_list;
1042 :
1043 0 : va_start(arg_list, fmt);
1044 0 : WriteV(name, labels, units, mults, fmt, arg_list, true);
1045 0 : va_end(arg_list);
1046 0 : }
1047 :
1048 85365233 : void AP_Logger::WriteV(const char *name, const char *labels, const char *units, const char *mults, const char *fmt, va_list arg_list,
1049 : bool is_critical, bool is_streaming)
1050 : {
1051 : // WriteV is not safe in replay as we can re-use IDs
1052 85365233 : const bool direct_comp = APM_BUILD_TYPE(APM_BUILD_Replay);
1053 85365233 : struct log_write_fmt *f = msg_fmt_for_name(name, labels, units, mults, fmt, direct_comp);
1054 85365338 : if (f == nullptr) {
1055 : // unable to map name to a messagetype; could be out of
1056 : // msgtypes, could be out of slots, ...
1057 : #if !APM_BUILD_TYPE(APM_BUILD_Replay)
1058 2 : INTERNAL_ERROR(AP_InternalError::error_t::logger_mapfailure);
1059 : #endif
1060 0 : return;
1061 : }
1062 :
1063 170730259 : for (uint8_t i=0; i<_next_backend; i++) {
1064 : va_list arg_copy;
1065 85365187 : va_copy(arg_copy, arg_list);
1066 85365187 : backends[i]->Write(f->msg_type, arg_copy, is_critical, is_streaming);
1067 85364923 : va_end(arg_copy);
1068 : }
1069 : }
1070 :
1071 : /*
1072 : when we are doing replay logging we want to delay start of the EKF
1073 : until after the headers are out so that on replay all parameter
1074 : values are available
1075 : */
1076 42719218 : bool AP_Logger::allow_start_ekf() const
1077 : {
1078 42719218 : if (!log_replay() || !log_while_disarmed()) {
1079 42542702 : return true;
1080 : }
1081 :
1082 352946 : for (uint8_t i=0; i<_next_backend; i++) {
1083 176516 : if (!backends[i]->allow_start_ekf()) {
1084 86 : return false;
1085 : }
1086 : }
1087 :
1088 176430 : return true;
1089 : }
1090 :
1091 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1092 85406824 : bool AP_Logger::assert_same_fmt_for_name(const AP_Logger::log_write_fmt *f,
1093 : const char *name,
1094 : const char *labels,
1095 : const char *units,
1096 : const char *mults,
1097 : const char *fmt) const
1098 : {
1099 85406824 : bool passed = true;
1100 85406824 : if (!streq(f->name, name)) {
1101 : // why exactly were we called?!
1102 0 : Debug("format names differ (%s) != (%s)", f->name, name);
1103 0 : passed = false;
1104 : }
1105 85406824 : if (!streq(f->labels, labels)) {
1106 0 : Debug("format labels differ (%s) vs (%s)", f->labels, labels);
1107 0 : passed = false;
1108 : }
1109 85406824 : if (!streq(f->fmt, fmt)) {
1110 0 : Debug("format fmt differ (%s) vs (%s)",
1111 : (f->fmt ? f->fmt : "nullptr"),
1112 : (fmt ? fmt : "nullptr"));
1113 0 : passed = false;
1114 : }
1115 : #if !APM_BUILD_TYPE(APM_BUILD_Replay)
1116 85406824 : if ((f->units != nullptr && units == nullptr) ||
1117 85406824 : (f->units == nullptr && units != nullptr) ||
1118 43229032 : (units !=nullptr && !streq(f->units, units))) {
1119 0 : Debug("format units differ (%s) vs (%s)",
1120 : (f->units ? f->units : "nullptr"),
1121 : (units ? units : "nullptr"));
1122 0 : passed = false;
1123 : }
1124 85406824 : if ((f->mults != nullptr && mults == nullptr) ||
1125 85406824 : (f->mults == nullptr && mults != nullptr) ||
1126 43229032 : (mults != nullptr && !streq(f->mults, mults))) {
1127 0 : Debug("format mults differ (%s) vs (%s)",
1128 : (f->mults ? f->mults : "nullptr"),
1129 : (mults ? mults : "nullptr"));
1130 0 : passed = false;
1131 : }
1132 85406824 : if (!passed) {
1133 0 : AP_BoardConfig::config_error("See console: Format definition must be consistent for every call of Write");
1134 : }
1135 : #endif
1136 85406824 : return passed;
1137 : }
1138 : #endif
1139 :
1140 85416182 : AP_Logger::log_write_fmt *AP_Logger::msg_fmt_for_name(const char *name, const char *labels, const char *units, const char *mults, const char *fmt, const bool direct_comp, const bool copy_strings)
1141 : {
1142 85416182 : WITH_SEMAPHORE(log_write_fmts_sem);
1143 : struct log_write_fmt *f;
1144 317000647 : for (f = log_write_fmts; f; f=f->next) {
1145 316991161 : if (!direct_comp) {
1146 316864597 : if (f->name == name) { // ptr comparison
1147 : // already have an ID for this name:
1148 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1149 85355875 : if (!assert_same_fmt_for_name(f, name, labels, units, mults, fmt)) {
1150 0 : return nullptr;
1151 : }
1152 : #endif
1153 85355875 : return f;
1154 : }
1155 : } else {
1156 : // direct comparison used from scripting where pointer is not maintained
1157 126564 : if (strcmp(f->name,name) == 0) {
1158 : // already have an ID for this name:
1159 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1160 50949 : if (!assert_same_fmt_for_name(f, name, labels, units, mults, fmt)) {
1161 0 : return nullptr;
1162 : }
1163 : #endif
1164 50949 : return f;
1165 : }
1166 : }
1167 : }
1168 :
1169 9486 : f = (struct log_write_fmt *)calloc(1, sizeof(*f));
1170 9486 : if (f == nullptr) {
1171 : // out of memory
1172 0 : return nullptr;
1173 : }
1174 : // no message type allocated for this name. Try to allocate one:
1175 9486 : int16_t msg_type = find_free_msg_type();
1176 9486 : if (msg_type == -1) {
1177 0 : free(f);
1178 0 : return nullptr;
1179 : }
1180 9486 : f->msg_type = msg_type;
1181 :
1182 9486 : if (copy_strings) {
1183 : // cannot use pointers to memory that might move, must allocate and copy
1184 14 : struct log_write_fmt_strings *ls_copy = (struct log_write_fmt_strings*)malloc(sizeof(log_write_fmt_strings));
1185 14 : if (ls_copy == nullptr) {
1186 0 : free(f);
1187 0 : return nullptr;
1188 : }
1189 :
1190 14 : strncpy_noterm(ls_copy->name, name, sizeof(ls_copy->name));
1191 14 : strncpy_noterm(ls_copy->format, fmt, sizeof(ls_copy->format));
1192 14 : strncpy_noterm(ls_copy->labels, labels, sizeof(ls_copy->labels));
1193 :
1194 14 : f->name = ls_copy->name;
1195 14 : f->fmt = ls_copy->format;
1196 14 : f->labels = ls_copy->labels;
1197 :
1198 14 : if (units != nullptr) {
1199 6 : strncpy_noterm(ls_copy->units, units, sizeof(ls_copy->units));
1200 6 : f->units = ls_copy->units;
1201 : }
1202 :
1203 14 : if (mults != nullptr) {
1204 6 : strncpy_noterm(ls_copy->multipliers, mults, sizeof(ls_copy->multipliers));
1205 6 : f->mults = ls_copy->multipliers;
1206 : }
1207 :
1208 : } else {
1209 9472 : f->name = name;
1210 9472 : f->fmt = fmt;
1211 9472 : f->labels = labels;
1212 9472 : f->units = units;
1213 9472 : f->mults = mults;
1214 : }
1215 :
1216 9486 : int16_t tmp = Write_calc_msg_len(fmt);
1217 9486 : if (tmp == -1) {
1218 0 : free(f);
1219 0 : return nullptr;
1220 : }
1221 :
1222 9486 : f->msg_len = tmp;
1223 :
1224 : // add direct_comp formats to start of list, otherwise add to the end, this minimises the number of string comparisons when walking the list in future calls
1225 9486 : if (direct_comp || (log_write_fmts == nullptr)) {
1226 2101 : f->next = log_write_fmts;
1227 2101 : log_write_fmts = f;
1228 : } else {
1229 7385 : struct log_write_fmt *list_end = log_write_fmts;
1230 21118 : while (list_end->next) {
1231 13733 : list_end=list_end->next;
1232 : }
1233 7385 : list_end->next = f;
1234 : }
1235 :
1236 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1237 9486 : struct log_write_fmt_strings ls_strings = {};
1238 9486 : struct LogStructure ls = {
1239 9486 : f->msg_type,
1240 9486 : f->msg_len,
1241 : ls_strings.name,
1242 : ls_strings.format,
1243 : ls_strings.labels,
1244 : ls_strings.units,
1245 : ls_strings.multipliers
1246 9486 : };
1247 9486 : memcpy((char*)ls_strings.name, f->name, MIN(sizeof(ls_strings.name), strlen(f->name)));
1248 9486 : memcpy((char*)ls_strings.format, f->fmt, MIN(sizeof(ls_strings.format), strlen(f->fmt)));
1249 9486 : memcpy((char*)ls_strings.labels, f->labels, MIN(sizeof(ls_strings.labels), strlen(f->labels)));
1250 9486 : if (f->units != nullptr) {
1251 7088 : memcpy((char*)ls_strings.units, f->units, MIN(sizeof(ls_strings.units), strlen(f->units)));
1252 : } else {
1253 2398 : memset((char*)ls_strings.units, '?', MIN(sizeof(ls_strings.format), strlen(f->fmt)));
1254 : }
1255 9486 : if (f->mults != nullptr) {
1256 7088 : memcpy((char*)ls_strings.multipliers, f->mults, MIN(sizeof(ls_strings.multipliers), strlen(f->mults)));
1257 : } else {
1258 2398 : memset((char*)ls_strings.multipliers, '?', MIN(sizeof(ls_strings.format), strlen(f->fmt)));
1259 : }
1260 9486 : if (!validate_structure(&ls, (int16_t)-1)) {
1261 0 : AP_BoardConfig::config_error("See console: Log structure invalid");
1262 : }
1263 : #endif
1264 :
1265 9486 : return f;
1266 85416310 : }
1267 :
1268 60062398 : const struct LogStructure *AP_Logger::structure_for_msg_type(const uint8_t msg_type) const
1269 : {
1270 6744191546 : for (uint16_t i=0; i<_num_types;i++) {
1271 6725686730 : const struct LogStructure *s = structure(i);
1272 6725675524 : if (s->msg_type == msg_type) {
1273 : // in use
1274 41546376 : return s;
1275 : }
1276 : }
1277 18504816 : return nullptr;
1278 : }
1279 :
1280 18506902 : const struct AP_Logger::log_write_fmt *AP_Logger::log_write_fmt_for_msg_type(const uint8_t msg_type) const
1281 : {
1282 : struct log_write_fmt *f;
1283 66832575 : for (f = log_write_fmts; f; f=f->next) {
1284 66832593 : if (f->msg_type == msg_type) {
1285 18506920 : return f;
1286 : }
1287 : }
1288 0 : return nullptr;
1289 : }
1290 :
1291 :
1292 : // returns true if the msg_type is already taken
1293 30708 : bool AP_Logger::msg_type_in_use(const uint8_t msg_type) const
1294 : {
1295 : // check static list of messages (e.g. from LOG_COMMON_STRUCTURES)
1296 : // check the write format types to see if we've used this one
1297 5892429 : for (uint16_t i=0; i<_num_types;i++) {
1298 5861721 : if (structure(i)->msg_type == msg_type) {
1299 : // in use
1300 0 : return true;
1301 : }
1302 : }
1303 :
1304 : struct log_write_fmt *f;
1305 86019 : for (f = log_write_fmts; f; f=f->next) {
1306 76533 : if (f->msg_type == msg_type) {
1307 21222 : return true;
1308 : }
1309 : }
1310 9486 : return false;
1311 : }
1312 :
1313 : // find a free message type
1314 9486 : int16_t AP_Logger::find_free_msg_type() const
1315 : {
1316 9486 : const uint8_t start = LOGGING_FIRST_DYNAMIC_MSGID;
1317 : // avoid using 255 here; perhaps we want to use it to extend things later
1318 30708 : for (uint16_t msg_type=start; msg_type>0; msg_type--) { // more likely to be free at end
1319 30708 : if (! msg_type_in_use(msg_type)) {
1320 9486 : return msg_type;
1321 : }
1322 : }
1323 0 : return -1;
1324 : }
1325 :
1326 : /*
1327 : * It is assumed that logstruct's char* variables are valid strings of
1328 : * maximum lengths for those fields (given in LogStructure.h e.g. LS_NAME_SIZE)
1329 : */
1330 21417 : bool AP_Logger::fill_logstructure(struct LogStructure &logstruct, const uint8_t msg_type) const
1331 : {
1332 : // check the static lists first...
1333 21417 : const LogStructure *found = structure_for_msg_type(msg_type);
1334 21417 : if (found != nullptr) {
1335 13267 : logstruct = *found;
1336 13267 : return true;
1337 : }
1338 :
1339 : // find log structure information corresponding to msg_type:
1340 : struct log_write_fmt *f;
1341 27621 : for (f = log_write_fmts; f; f=f->next) {
1342 27621 : if (f->msg_type == msg_type) {
1343 8150 : break;
1344 : }
1345 : }
1346 :
1347 8150 : if (!f) {
1348 0 : return false;
1349 : }
1350 :
1351 8150 : logstruct.msg_type = msg_type;
1352 8150 : strncpy((char*)logstruct.name, f->name, LS_NAME_SIZE);
1353 8150 : strncpy((char*)logstruct.format, f->fmt, LS_FORMAT_SIZE);
1354 8150 : strncpy((char*)logstruct.labels, f->labels, LS_LABELS_SIZE);
1355 8150 : if (f->units != nullptr) {
1356 5833 : strncpy((char*)logstruct.units, f->units, LS_UNITS_SIZE);
1357 : } else {
1358 2317 : memset((char*)logstruct.units, '\0', LS_UNITS_SIZE);
1359 2317 : memset((char*)logstruct.units, '?', MIN(LS_UNITS_SIZE,strlen(logstruct.format)));
1360 : }
1361 8150 : if (f->mults != nullptr) {
1362 5833 : strncpy((char*)logstruct.multipliers, f->mults, LS_MULTIPLIERS_SIZE);
1363 : } else {
1364 2317 : memset((char*)logstruct.multipliers, '\0', LS_MULTIPLIERS_SIZE);
1365 2317 : memset((char*)logstruct.multipliers, '?', MIN(LS_MULTIPLIERS_SIZE, strlen(logstruct.format)));
1366 : // special magic to set units/mults for TimeUS, by far and
1367 : // away the most common first field
1368 2317 : if (!strncmp(logstruct.labels, "TimeUS,", MIN(LS_LABELS_SIZE, strlen("TimeUS,")))) {
1369 2317 : ((char*)(logstruct.units))[0] = 's';
1370 2317 : ((char*)(logstruct.multipliers))[0] = 'F';
1371 : }
1372 : }
1373 8150 : logstruct.msg_len = f->msg_len;
1374 8150 : return true;
1375 : }
1376 :
1377 : /* calculate the length of output of a format string. Note that this
1378 : * returns an int16_t; if it returns -1 then an error has occurred.
1379 : * This was mechanically converted from init_field_types in
1380 : * Tools/Replay/MsgHandler.cpp */
1381 467538 : int16_t AP_Logger::Write_calc_msg_len(const char *fmt) const
1382 : {
1383 467538 : size_t len = LOG_PACKET_HEADER_LEN;
1384 4564629 : for (size_t i=0; i<strlen(fmt); i++) {
1385 4097091 : switch(fmt[i]) {
1386 6282 : case 'a' : len += sizeof(int16_t[32]); break;
1387 42911 : case 'b' : len += sizeof(int8_t); break;
1388 182124 : case 'c' : len += sizeof(int16_t); break;
1389 25196 : case 'd' : len += sizeof(double); break;
1390 30023 : case 'e' : len += sizeof(int32_t); break;
1391 1885520 : case 'f' : len += sizeof(float); break;
1392 0 : case 'g' : len += sizeof(float16_s); break;
1393 75118 : case 'h' : len += sizeof(int16_t); break;
1394 31116 : case 'i' : len += sizeof(int32_t); break;
1395 7118 : case 'n' : len += sizeof(char[4]); break;
1396 693092 : case 'B' : len += sizeof(uint8_t); break;
1397 33504 : case 'C' : len += sizeof(uint16_t); break;
1398 0 : case 'E' : len += sizeof(uint32_t); break;
1399 315995 : case 'H' : len += sizeof(uint16_t); break;
1400 203692 : case 'I' : len += sizeof(uint32_t); break;
1401 106158 : case 'L' : len += sizeof(int32_t); break;
1402 2094 : case 'M' : len += sizeof(uint8_t); break;
1403 16830 : case 'N' : len += sizeof(char[16]); break;
1404 16754 : case 'Z' : len += sizeof(char[64]); break;
1405 0 : case 'q' : len += sizeof(int64_t); break;
1406 423564 : case 'Q' : len += sizeof(uint64_t); break;
1407 0 : default:
1408 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1409 0 : AP_HAL::panic("Unknown format specifier (%c)", fmt[i]);
1410 : #endif
1411 0 : return -1;
1412 : }
1413 : }
1414 467538 : if (len > LOG_PACKET_MAX_LEN) {
1415 0 : return -1;
1416 : }
1417 467538 : return (int16_t)len;
1418 : }
1419 :
1420 : /*
1421 : see if we need to save a crash dump. Returns true if either no crash
1422 : dump available or we have saved it to sdcard. This is called
1423 : continuously until success to account for late mount of the microSD
1424 : */
1425 1717 : bool AP_Logger::check_crash_dump_save(void)
1426 : {
1427 : #if defined(AP_CRASHDUMP_FATFS_ENABLED) && AP_CRASHDUMP_FATFS_ENABLED
1428 : if (hal.util->last_crash_dump_size() > 0) {
1429 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Previous CrashDump: APM/CrashDump.DAT");
1430 : return true;
1431 : }
1432 : // The SD card may not be mounted yet, so keep checking.
1433 : return false;
1434 : #else
1435 1717 : int fd = AP::FS().open("@SYS/crash_dump.bin", O_RDONLY);
1436 1717 : if (fd == -1) {
1437 : // we don't have a crash dump file. The @SYS filesystem
1438 : // returns -1 for open on empty files
1439 1717 : return true;
1440 : }
1441 0 : int fd2 = AP::FS().open("APM/crash_dump.bin", O_WRONLY|O_CREAT|O_TRUNC);
1442 0 : if (fd2 == -1) {
1443 : // sdcard not available yet, try again later
1444 0 : AP::FS().close(fd);
1445 0 : return false;
1446 : }
1447 : uint8_t buf[128];
1448 : int32_t n;
1449 0 : while ((n = AP::FS().read(fd, buf, sizeof(buf))) > 0) {
1450 0 : AP::FS().write(fd2, buf, n);
1451 : }
1452 0 : AP::FS().close(fd2);
1453 0 : AP::FS().close(fd);
1454 0 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Saved crash_dump.bin");
1455 0 : return true;
1456 : #endif
1457 : }
1458 :
1459 : // thread for processing IO - in general IO involves a long blocking DMA write to an SPI device
1460 : // and the thread will sleep while this completes preventing other tasks from running, it therefore
1461 : // is necessary to run the IO in it's own thread
1462 2094 : void AP_Logger::io_thread(void)
1463 : {
1464 2094 : uint32_t last_run_us = AP_HAL::micros();
1465 2094 : uint32_t last_stack_us = last_run_us;
1466 2094 : uint32_t last_crash_check_us = last_run_us;
1467 2094 : bool done_crash_dump_save = false;
1468 :
1469 : while (true) {
1470 52999716 : uint32_t now = AP_HAL::micros();
1471 :
1472 52999716 : uint32_t delay = 250U; // always have some delay
1473 52999716 : if (now - last_run_us < 1000) {
1474 52957283 : delay = MAX(1000 - (now - last_run_us), delay);
1475 : }
1476 52999716 : hal.scheduler->delay_microseconds(delay);
1477 :
1478 52997622 : last_run_us = AP_HAL::micros();
1479 :
1480 105995244 : FOR_EACH_BACKEND(io_timer());
1481 :
1482 52997622 : if (now - last_stack_us > 100000U) {
1483 1520757 : last_stack_us = now;
1484 1520757 : hal.util->log_stack_info();
1485 : }
1486 :
1487 : // check for saving a crash dump file every 5s
1488 52997622 : if (!done_crash_dump_save &&
1489 2341507 : now - last_crash_check_us > 5000000U) {
1490 1717 : last_crash_check_us = now;
1491 1717 : done_crash_dump_save = check_crash_dump_save();
1492 : }
1493 : #if HAL_LOGGER_FILE_CONTENTS_ENABLED
1494 52997622 : file_content_update();
1495 : #endif
1496 52997622 : }
1497 : }
1498 :
1499 : // start the update thread
1500 2094 : void AP_Logger::start_io_thread(void)
1501 : {
1502 2094 : WITH_SEMAPHORE(_log_send_sem);
1503 :
1504 2094 : if (_io_thread_started) {
1505 0 : return;
1506 : }
1507 :
1508 2094 : if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Logger::io_thread, void), "log_io", HAL_LOGGING_STACK_SIZE, AP_HAL::Scheduler::PRIORITY_IO, 1)) {
1509 0 : AP_HAL::panic("Failed to start Logger IO thread");
1510 : }
1511 :
1512 2094 : _io_thread_started = true;
1513 2094 : return;
1514 2094 : }
1515 :
1516 : /* End of Write support */
1517 :
1518 : #undef FOR_EACH_BACKEND
1519 :
1520 : // Wrote an event packet
1521 39494 : void AP_Logger::Write_Event(LogEvent id)
1522 : {
1523 39494 : const struct log_Event pkt{
1524 : LOG_PACKET_HEADER_INIT(LOG_EVENT_MSG),
1525 39494 : time_us : AP_HAL::micros64(),
1526 : id : (uint8_t)id
1527 39494 : };
1528 39494 : WriteCriticalBlock(&pkt, sizeof(pkt));
1529 39494 : }
1530 :
1531 : // Write an error packet
1532 4323 : void AP_Logger::Write_Error(LogErrorSubsystem sub_system,
1533 : LogErrorCode error_code)
1534 : {
1535 4323 : const struct log_Error pkt{
1536 : LOG_PACKET_HEADER_INIT(LOG_ERROR_MSG),
1537 4323 : time_us : AP_HAL::micros64(),
1538 : sub_system : uint8_t(sub_system),
1539 : error_code : uint8_t(error_code),
1540 4323 : };
1541 4323 : WriteCriticalBlock(&pkt, sizeof(pkt));
1542 4323 : }
1543 :
1544 : /*
1545 : return true if we are in a logging persistance state, where we keep
1546 : logging after a disarm or an arming failure
1547 : */
1548 93072729 : bool AP_Logger::in_log_persistance(void) const
1549 : {
1550 93072729 : uint32_t now = AP_HAL::millis();
1551 93072655 : uint32_t persist_ms = HAL_LOGGER_ARM_PERSIST*1000U;
1552 93072655 : if (_force_long_log_persist) {
1553 : // log for 10x longer than default
1554 120385 : persist_ms *= 10U;
1555 : }
1556 :
1557 : // keep logging for HAL_LOGGER_ARM_PERSIST seconds after disarming
1558 93072655 : const uint32_t arm_change_ms = hal.util->get_last_armed_change();
1559 93072619 : if (!hal.util->get_soft_armed() && arm_change_ms != 0 && now - arm_change_ms < persist_ms) {
1560 13478190 : return true;
1561 : }
1562 :
1563 : // keep logging for HAL_LOGGER_ARM_PERSIST seconds after an arming failure
1564 79594398 : if (_last_arming_failure_ms && now - _last_arming_failure_ms < persist_ms) {
1565 494229 : return true;
1566 : }
1567 :
1568 79100169 : return false;
1569 : }
1570 :
1571 :
1572 : /*
1573 : return true if we should log while disarmed
1574 : */
1575 231149809 : bool AP_Logger::log_while_disarmed(void) const
1576 : {
1577 231149809 : if (_force_log_disarmed) {
1578 0 : return true;
1579 : }
1580 261108588 : if (_params.log_disarmed == LogDisarmed::LOG_WHILE_DISARMED ||
1581 261108584 : _params.log_disarmed == LogDisarmed::LOG_WHILE_DISARMED_DISCARD ||
1582 29958787 : (_params.log_disarmed == LogDisarmed::LOG_WHILE_DISARMED_NOT_USB && !hal.gpio->usb_connected())) {
1583 201191021 : return true;
1584 : }
1585 :
1586 29958783 : return in_log_persistance();
1587 : }
1588 :
1589 : #if HAL_LOGGER_FILE_CONTENTS_ENABLED
1590 1276 : void AP_Logger::prepare_at_arming_sys_file_logging()
1591 : {
1592 : // free existing content:
1593 1276 : at_arm_file_content.reset();
1594 :
1595 : /*
1596 : log files useful for diagnostics on arming. We log on arming as
1597 : with LOG_DISARMED we don't want to log the statistics at boot or
1598 : we wouldn't get a realistic idea of key system values
1599 : Note that some of these files may not exist, in that case they
1600 : are ignored
1601 : */
1602 : static const char *log_content_filenames[] = {
1603 : "@SYS/uarts.txt",
1604 : #ifdef HAL_DEBUG_BUILD
1605 : // logging dma.txt has a performance impact
1606 : "@SYS/dma.txt",
1607 : #endif
1608 : "@SYS/memory.txt",
1609 : "@SYS/threads.txt",
1610 : "@SYS/timers.txt",
1611 : "@ROMFS/hwdef.dat",
1612 : "@SYS/storage.bin",
1613 : "@SYS/crash_dump.bin",
1614 : "@ROMFS/defaults.parm",
1615 : "APM/CrashDump.DAT",
1616 : };
1617 14036 : for (const auto *name : log_content_filenames) {
1618 12760 : log_file_content(at_arm_file_content, name);
1619 : }
1620 1276 : }
1621 :
1622 1276 : void AP_Logger::FileContent::reset()
1623 : {
1624 1276 : WITH_SEMAPHORE(sem);
1625 1276 : file_list *next = nullptr;
1626 2139 : for (auto *c = head; c != nullptr; c = next) {
1627 863 : next = c->next;
1628 863 : delete [] c->filename;
1629 863 : delete c;
1630 : }
1631 1276 : head = nullptr;
1632 1276 : tail = nullptr;
1633 1276 : if (fd != -1) {
1634 191 : AP::FS().close(fd);
1635 191 : fd = -1;
1636 : }
1637 1276 : counter = 0;
1638 1276 : fast = false;
1639 1276 : offset = 0;
1640 1276 : }
1641 :
1642 : // removes victim from FileContent ***and delete()s it***
1643 11711 : void AP_Logger::FileContent::remove_and_free(file_list *victim)
1644 : {
1645 11711 : WITH_SEMAPHORE(sem);
1646 :
1647 11711 : file_list *prev = nullptr;
1648 11711 : for (auto *c = head; c != nullptr; prev = c, c = c->next) {
1649 11711 : if (c != victim) {
1650 0 : continue;
1651 : }
1652 :
1653 : // found the item to remove; remove it and return
1654 11711 : if (prev == nullptr) {
1655 11711 : head = victim->next;
1656 : } else {
1657 0 : prev->next = victim->next;
1658 : }
1659 11711 : delete [] victim->filename;
1660 11711 : delete victim;
1661 11711 : return;
1662 : }
1663 11711 : }
1664 :
1665 :
1666 : /*
1667 : log the content of a file in FILE log messages
1668 : */
1669 93 : void AP_Logger::log_file_content(const char *filename)
1670 : {
1671 93 : log_file_content(normal_file_content, filename);
1672 93 : }
1673 :
1674 12853 : void AP_Logger::log_file_content(FileContent &file_content, const char *filename)
1675 : {
1676 12853 : WITH_SEMAPHORE(file_content.sem);
1677 12853 : auto *file = NEW_NOTHROW file_list;
1678 12853 : if (file == nullptr) {
1679 0 : return;
1680 : }
1681 : // make copy to allow original to go out of scope
1682 12853 : const size_t len = strlen(filename)+1;
1683 12853 : char * tmp_filename = NEW_NOTHROW char[len];
1684 12853 : if (tmp_filename == nullptr) {
1685 0 : delete file;
1686 0 : return;
1687 : }
1688 12853 : strncpy(tmp_filename, filename, len);
1689 12853 : file->filename = tmp_filename;
1690 : // Remove directory if whole file name will not fit
1691 12853 : const char * name = strrchr(file->filename, '/');
1692 12853 : if ((len-1 > sizeof(file->log_filename)) && (name != nullptr)) {
1693 3921 : strncpy_noterm(file->log_filename, name+1, sizeof(file->log_filename));
1694 : } else {
1695 8932 : strncpy_noterm(file->log_filename, file->filename, sizeof(file->log_filename));
1696 : }
1697 12853 : if (file_content.head == nullptr) {
1698 1356 : file_content.tail = file_content.head = file;
1699 1356 : file_content.fd = -1;
1700 : } else {
1701 11497 : file_content.tail->next = file;
1702 11497 : file_content.tail = file;
1703 : }
1704 12853 : }
1705 :
1706 : /*
1707 : periodic call to log file content
1708 : */
1709 52997622 : void AP_Logger::file_content_update(void)
1710 : {
1711 52997622 : if (file_content_prepare_for_arming) {
1712 1276 : file_content_prepare_for_arming = false;
1713 1276 : prepare_at_arming_sys_file_logging();
1714 : }
1715 :
1716 52997622 : file_content_update(at_arm_file_content);
1717 52997622 : file_content_update(normal_file_content);
1718 52997622 : }
1719 :
1720 105995244 : void AP_Logger::file_content_update(FileContent &file_content)
1721 : {
1722 105995244 : auto *file = file_content.head;
1723 105995244 : if (file == nullptr) {
1724 105665461 : return;
1725 : }
1726 :
1727 : /* this function is called at around 100Hz on average (tested on
1728 : 400Hz copter). We don't want to saturate the logging with file
1729 : data, so we reduce the frequency of 64 byte file writes by a
1730 : factor of 10. For crash dump files we dump 10x faster so
1731 : we get it in a reasonable time (full dump of 450k in about 1
1732 : minute)
1733 : */
1734 3356980 : file_content.counter++;
1735 3356980 : const uint8_t frequency = file_content.fast?1:10;
1736 3356980 : if (file_content.counter % frequency != 0) {
1737 3015486 : return;
1738 : }
1739 :
1740 341494 : if (file_content.fd == -1) {
1741 : // open a new file
1742 11986 : file_content.fd = AP::FS().open(file->filename, O_RDONLY);
1743 22956 : file_content.fast = strncmp(file->filename, "@SYS/crash_dump", 15) == 0 ||
1744 10970 : strcmp(file->filename, "APM/CrashDump.DAT") == 0;
1745 11986 : if (file_content.fd == -1) {
1746 9361 : file_content.remove_and_free(file);
1747 9361 : return;
1748 : }
1749 2625 : file_content.offset = 0;
1750 2625 : if (file_content.fast) {
1751 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Logging %s", file->filename);
1752 : }
1753 : }
1754 :
1755 332133 : struct log_File pkt {
1756 : LOG_PACKET_HEADER_INIT(LOG_FILE_MSG),
1757 : };
1758 332133 : memcpy(pkt.filename, file->log_filename, sizeof(pkt.filename));
1759 332133 : const auto length = AP::FS().read(file_content.fd, pkt.data, sizeof(pkt.data));
1760 332133 : if (length <= 0) {
1761 2350 : AP::FS().close(file_content.fd);
1762 2350 : file_content.fd = -1;
1763 2350 : file_content.remove_and_free(file);
1764 2350 : return;
1765 : }
1766 329783 : pkt.offset = file_content.offset;
1767 329783 : pkt.length = length;
1768 329783 : if (WriteBlock_first_succeed(&pkt, sizeof(pkt))) {
1769 315470 : file_content.offset += length;
1770 : } else {
1771 : // seek back ready for another try
1772 14313 : AP::FS().lseek(file_content.fd, file_content.offset, SEEK_SET);
1773 : }
1774 : }
1775 : #endif // HAL_LOGGER_FILE_CONTENTS_ENABLED
1776 :
1777 : namespace AP {
1778 :
1779 694656197 : AP_Logger &logger()
1780 : {
1781 694656197 : return *AP_Logger::get_singleton();
1782 : }
1783 :
1784 : };
1785 :
1786 : #endif // HAL_LOGGING_ENABLED
|
