Line data Source code
1 : #include "AP_Logger_config.h"
2 :
3 : #if HAL_LOGGING_ENABLED
4 :
5 : #include "AP_Logger_Backend.h"
6 :
7 : #include "LoggerMessageWriter.h"
8 :
9 : #include "AP_Common/AP_FWVersion.h"
10 : #include <AP_InternalError/AP_InternalError.h>
11 : #include <AP_Scheduler/AP_Scheduler.h>
12 : #include <AP_Rally/AP_Rally.h>
13 : #include <AP_Vehicle/AP_Vehicle_Type.h>
14 : #include <Filter/Filter.h>
15 : #include "AP_Logger.h"
16 : #include <AP_IOMCU/AP_IOMCU.h>
17 :
18 : #if HAL_LOGGER_FENCE_ENABLED
19 : #include <AC_Fence/AC_Fence.h>
20 : #endif
21 :
22 : extern const AP_HAL::HAL& hal;
23 :
24 2094 : AP_Logger_Backend::AP_Logger_Backend(AP_Logger &front,
25 2094 : class LoggerMessageWriter_DFLogStart *writer) :
26 2094 : _front(front),
27 2094 : _startup_messagewriter(writer)
28 : {
29 2094 : writer->set_logger_backend(this);
30 2094 : }
31 :
32 2483489 : uint8_t AP_Logger_Backend::num_types() const
33 : {
34 2483489 : return _front._num_types;
35 : }
36 :
37 2479460 : const struct LogStructure *AP_Logger_Backend::structure(uint8_t num) const
38 : {
39 2479460 : return _front.structure(num);
40 : }
41 :
42 93742 : uint8_t AP_Logger_Backend::num_units() const
43 : {
44 93742 : return _front._num_units;
45 : }
46 :
47 78297 : const struct UnitStructure *AP_Logger_Backend::unit(uint8_t num) const
48 : {
49 78297 : return _front.unit(num);
50 : }
51 :
52 46301 : uint8_t AP_Logger_Backend::num_multipliers() const
53 : {
54 46301 : return _front._num_multipliers;
55 : }
56 :
57 30856 : const struct MultiplierStructure *AP_Logger_Backend::multiplier(uint8_t num) const
58 : {
59 30856 : return _front.multiplier(num);
60 : }
61 :
62 10368 : AP_Logger_Backend::vehicle_startup_message_Writer AP_Logger_Backend::vehicle_message_writer() const {
63 10368 : return _front._vehicle_messages;
64 : }
65 :
66 1402375 : void AP_Logger_Backend::periodic_10Hz(const uint32_t now)
67 : {
68 1402375 : }
69 :
70 150292 : void AP_Logger_Backend::periodic_1Hz()
71 : {
72 150292 : if (_rotate_pending && !logging_enabled()) {
73 18 : _rotate_pending = false;
74 : // handle log rotation once we stop logging
75 18 : stop_logging_async();
76 : }
77 150292 : df_stats_log();
78 150292 : }
79 :
80 119066 : void AP_Logger_Backend::periodic_fullrate()
81 : {
82 119066 : push_log_blocks();
83 119066 : }
84 :
85 41684408 : void AP_Logger_Backend::periodic_tasks()
86 : {
87 41684408 : uint32_t now = AP_HAL::millis();
88 41684408 : if (now - _last_periodic_1Hz > 1000) {
89 150426 : periodic_1Hz();
90 150426 : _last_periodic_1Hz = now;
91 : }
92 41684408 : if (now - _last_periodic_10Hz > 100) {
93 1407065 : periodic_10Hz(now);
94 1407065 : _last_periodic_10Hz = now;
95 : }
96 41684408 : periodic_fullrate();
97 41684408 : }
98 :
99 2079 : void AP_Logger_Backend::start_new_log_reset_variables()
100 : {
101 2079 : _dropped = 0;
102 2079 : _startup_messagewriter->reset();
103 2079 : _front.backend_starting_new_log(this);
104 2079 : _formats_written.clearall();
105 2079 : }
106 :
107 : // We may need to make sure data is loggable before starting the
108 : // EKF; when allow_start_ekf we should be able to log that data
109 176516 : bool AP_Logger_Backend::allow_start_ekf() const
110 : {
111 176516 : if (!_startup_messagewriter->fmt_done()) {
112 86 : return false;
113 : }
114 : // we need to push all startup messages out, or the code in
115 : // WriteBlockCheckStartupMessages bites us.
116 176430 : if (!_startup_messagewriter->finished()) {
117 0 : return false;
118 : }
119 176430 : return true;
120 : }
121 :
122 : // this method can be overridden to do extra things with your buffer.
123 : // for example, in AP_Logger_MAVLink we may push messages into the UART.
124 41677663 : void AP_Logger_Backend::push_log_blocks() {
125 41677663 : WriteMoreStartupMessages();
126 41677663 : }
127 :
128 : // source more messages from the startup message writer:
129 41681431 : void AP_Logger_Backend::WriteMoreStartupMessages()
130 : {
131 : #if APM_BUILD_TYPE(APM_BUILD_Replay)
132 : return;
133 : #endif
134 :
135 41681431 : if (_startup_messagewriter->finished()) {
136 39952165 : return;
137 : }
138 :
139 1729266 : _writing_startup_messages = true;
140 1729266 : _startup_messagewriter->process();
141 1729266 : _writing_startup_messages = false;
142 : }
143 :
144 : /*
145 : * support for Write():
146 : */
147 :
148 :
149 : // output a FMT message if not already done so
150 50963 : void AP_Logger_Backend::Safe_Write_Emit_FMT(uint8_t msg_type)
151 : {
152 50963 : if (have_emitted_format_for_type(LogMessages(msg_type))) {
153 50949 : return;
154 : }
155 14 : Write_Emit_FMT(msg_type);
156 : }
157 :
158 21417 : bool AP_Logger_Backend::Write_Emit_FMT(uint8_t msg_type)
159 : {
160 : #if APM_BUILD_TYPE(APM_BUILD_Replay)
161 : if (msg_type < REPLAY_LOG_NEW_MSG_MIN || msg_type > REPLAY_LOG_NEW_MSG_MAX) {
162 : // don't re-emit FMU msgs unless they are in the replay range
163 : return true;
164 : }
165 : #endif
166 :
167 : // get log structure from front end:
168 21417 : struct AP_Logger::log_write_fmt_strings ls = {};
169 21417 : struct LogStructure logstruct = {
170 : // these will be overwritten, but need to keep the compiler happy:
171 : 0,
172 : 0,
173 : ls.name,
174 : ls.format,
175 : ls.labels,
176 : ls.units,
177 : ls.multipliers
178 21417 : };
179 21417 : if (!_front.fill_logstructure(logstruct, msg_type)) {
180 : // this is a bug; we've been asked to write out the FMT
181 : // message for a msg_type, but the frontend can't supply the
182 : // required information
183 0 : INTERNAL_ERROR(AP_InternalError::error_t::logger_missing_logstructure);
184 0 : return false;
185 : }
186 :
187 21417 : if (!Write_Format(&logstruct)) {
188 126 : return false;
189 : }
190 21291 : if (!Write_Format_Units(&logstruct)) {
191 0 : return false;
192 : }
193 :
194 21291 : return true;
195 : }
196 :
197 85365182 : bool AP_Logger_Backend::Write(const uint8_t msg_type, va_list arg_list, bool is_critical, bool is_streaming)
198 : {
199 : // stack-allocate a buffer so we can WriteBlock(); this could be
200 : // 255 bytes! If we were willing to lose the WriteBlock
201 : // abstraction we could do WriteBytes() here instead?
202 85365182 : const char *fmt = nullptr;
203 : uint8_t msg_len;
204 : AP_Logger::log_write_fmt *f;
205 316873849 : for (f = _front.log_write_fmts; f; f=f->next) {
206 316873853 : if (f->msg_type == msg_type) {
207 85365186 : fmt = f->fmt;
208 85365186 : msg_len = f->msg_len;
209 85365186 : break;
210 : }
211 : }
212 85365182 : if (fmt == nullptr) {
213 0 : INTERNAL_ERROR(AP_InternalError::error_t::logger_logwrite_missingfmt);
214 0 : return false;
215 : }
216 85365182 : if (bufferspace_available() < msg_len) {
217 3057 : return false;
218 : }
219 :
220 85362055 : uint8_t buffer[msg_len];
221 85362055 : uint8_t offset = 0;
222 85362055 : buffer[offset++] = HEAD_BYTE1;
223 85362055 : buffer[offset++] = HEAD_BYTE2;
224 85362055 : buffer[offset++] = msg_type;
225 785910489 : for (uint8_t i=0; i<strlen(fmt); i++) {
226 700548484 : uint8_t charlen = 0;
227 700548484 : switch(fmt[i]) {
228 6381 : case 'b': {
229 6381 : int8_t tmp = va_arg(arg_list, int);
230 6381 : memcpy(&buffer[offset], &tmp, sizeof(int8_t));
231 6381 : offset += sizeof(int8_t);
232 6381 : break;
233 : }
234 3292 : case 'h':
235 : case 'c': {
236 3292 : int16_t tmp = va_arg(arg_list, int);
237 3292 : memcpy(&buffer[offset], &tmp, sizeof(int16_t));
238 3292 : offset += sizeof(int16_t);
239 3292 : break;
240 : }
241 125108873 : case 'd': {
242 125108873 : double tmp = va_arg(arg_list, double);
243 125108873 : memcpy(&buffer[offset], &tmp, sizeof(double));
244 125108873 : offset += sizeof(double);
245 125108873 : break;
246 : }
247 82998 : case 'i':
248 : case 'L':
249 : case 'e': {
250 82998 : int32_t tmp = va_arg(arg_list, int);
251 82998 : memcpy(&buffer[offset], &tmp, sizeof(int32_t));
252 82998 : offset += sizeof(int32_t);
253 82998 : break;
254 : }
255 260409942 : case 'f': {
256 260409942 : float tmp = va_arg(arg_list, double);
257 260409910 : memcpy(&buffer[offset], &tmp, sizeof(float));
258 260409910 : offset += sizeof(float);
259 260409910 : break;
260 : }
261 0 : case 'g': {
262 : Float16_t tmp;
263 0 : tmp.set(va_arg(arg_list, double));;
264 0 : memcpy(&buffer[offset], &tmp, sizeof(tmp));
265 0 : offset += sizeof(tmp);
266 0 : break;
267 : }
268 0 : case 'n':
269 0 : charlen = 4;
270 0 : break;
271 150692509 : case 'M':
272 : case 'B': {
273 150692509 : uint8_t tmp = va_arg(arg_list, int);
274 150692499 : memcpy(&buffer[offset], &tmp, sizeof(uint8_t));
275 150692499 : offset += sizeof(uint8_t);
276 150692499 : break;
277 : }
278 186147 : case 'H':
279 : case 'C': {
280 186147 : uint16_t tmp = va_arg(arg_list, int);
281 186147 : memcpy(&buffer[offset], &tmp, sizeof(uint16_t));
282 186147 : offset += sizeof(uint16_t);
283 186147 : break;
284 : }
285 78662607 : case 'I':
286 : case 'E': {
287 78662607 : uint32_t tmp = va_arg(arg_list, uint32_t);
288 78662607 : memcpy(&buffer[offset], &tmp, sizeof(uint32_t));
289 78662607 : offset += sizeof(uint32_t);
290 78662607 : break;
291 : }
292 35852 : case 'N':
293 35852 : charlen = 16;
294 35852 : break;
295 48 : case 'Z':
296 48 : charlen = 64;
297 48 : break;
298 0 : case 'q': {
299 0 : int64_t tmp = va_arg(arg_list, int64_t);
300 0 : memcpy(&buffer[offset], &tmp, sizeof(int64_t));
301 0 : offset += sizeof(int64_t);
302 0 : break;
303 : }
304 85362029 : case 'Q': {
305 85362029 : uint64_t tmp = va_arg(arg_list, uint64_t);
306 85362021 : memcpy(&buffer[offset], &tmp, sizeof(uint64_t));
307 85362021 : offset += sizeof(uint64_t);
308 85362021 : break;
309 : }
310 0 : case 'a': {
311 0 : int16_t *tmp = va_arg(arg_list, int16_t*);
312 0 : const uint8_t bytes = 32*2;
313 0 : memcpy(&buffer[offset], tmp, bytes);
314 0 : offset += bytes;
315 0 : break;
316 : }
317 : }
318 700548434 : if (charlen != 0) {
319 35900 : char *tmp = va_arg(arg_list, char*);
320 35900 : uint8_t len = strnlen(tmp, charlen);
321 35900 : memcpy(&buffer[offset], tmp, len);
322 35900 : memset(&buffer[offset+len], 0, charlen-len);
323 35900 : offset += charlen;
324 : }
325 : }
326 :
327 85362005 : return WritePrioritisedBlock(buffer, msg_len, is_critical, is_streaming);
328 85365182 : }
329 :
330 1857824 : bool AP_Logger_Backend::StartNewLogOK() const
331 : {
332 1857824 : if (logging_started()) {
333 1857695 : return false;
334 : }
335 130 : if (_front._log_bitmask == 0) {
336 0 : return false;
337 : }
338 130 : if (_front.in_log_download()) {
339 0 : return false;
340 : }
341 130 : if (!hal.scheduler->in_main_thread()) {
342 35 : return false;
343 : }
344 95 : return true;
345 : }
346 :
347 : // validate that pBuffer looks like a message, extract message type.
348 : // Returns false if this doesn't look like a valid message.
349 159107966 : bool AP_Logger_Backend::message_type_from_block(const void *pBuffer, uint16_t size, LogMessages &type) const
350 : {
351 159107966 : if (size < 3) {
352 0 : return false;
353 : }
354 159107966 : if (((uint8_t*)pBuffer)[0] != HEAD_BYTE1 ||
355 159107983 : ((uint8_t*)pBuffer)[1] != HEAD_BYTE2) {
356 : // Not passed a message
357 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
358 0 : return false;
359 : }
360 159107994 : type = LogMessages(((uint8_t*)pBuffer)[2]);
361 159107994 : return true;
362 : }
363 :
364 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
365 266859330 : void AP_Logger_Backend::validate_WritePrioritisedBlock(const void *pBuffer,
366 : uint16_t size)
367 : {
368 : // just check the first few packets to avoid too much overhead
369 : // (finding the structures is expensive)
370 : static uint16_t count = 0;
371 266859330 : if (count > 65534) {
372 206899494 : return;
373 : }
374 59959836 : count++;
375 :
376 : // we assume here that we ever WritePrioritisedBlock for a single
377 : // message. If this assumption becomes false we can't do these
378 : // checks.
379 59959836 : if (size < 3) {
380 0 : AP_HAL::panic("Short prioritised block");
381 : }
382 : LogMessages type;
383 59959836 : if (!message_type_from_block(pBuffer, size, type)) {
384 0 : AP_HAL::panic("Not passed a message");
385 : }
386 : uint8_t type_len;
387 : const char *name_src;
388 59959833 : const struct LogStructure *s = _front.structure_for_msg_type(type);
389 59959803 : if (s == nullptr) {
390 18506837 : const struct AP_Logger::log_write_fmt *t = _front.log_write_fmt_for_msg_type(type);
391 18506919 : if (t == nullptr) {
392 0 : AP_HAL::panic("No structure for msg_type=%u", type);
393 : }
394 18506921 : type_len = t->msg_len;
395 18506921 : name_src = t->name;
396 : } else {
397 41452966 : type_len = s->msg_len;
398 41452966 : name_src = s->name;
399 : }
400 59959887 : if (type_len != size) {
401 0 : char name[5] = {}; // get a null-terminated string
402 0 : if (name_src != nullptr) {
403 0 : memcpy(name, name_src, 4);
404 : } else {
405 0 : strncpy(name, "?NM?", ARRAY_SIZE(name));
406 : }
407 0 : AP_HAL::panic("Size mismatch for %u (%s) (expected=%u got=%u)",
408 : type, name, type_len, size);
409 : }
410 : }
411 : #endif
412 :
413 99148267 : bool AP_Logger_Backend::ensure_format_emitted(const void *pBuffer, uint16_t size)
414 : {
415 : #if APM_BUILD_TYPE(APM_BUILD_Replay)
416 : // we trust that Replay will correctly emit formats as required
417 : return true;
418 : #endif
419 :
420 : // extract the ID:
421 : LogMessages type;
422 99148267 : if (!message_type_from_block(pBuffer, size, type)) {
423 0 : return false;
424 : }
425 99148260 : if (have_emitted_format_for_type(type)) {
426 99126122 : return true;
427 : }
428 :
429 : // make sure the FMT message has gone out!
430 22144 : if (type == LOG_FORMAT_MSG) {
431 : // kind of? Our caller is just about to emit this....
432 2019 : return true;
433 : }
434 21415 : if (!have_emitted_format_for_type(LOG_FORMAT_MSG) &&
435 1290 : !Write_Emit_FMT(LOG_FORMAT_MSG)) {
436 0 : return false;
437 : }
438 :
439 20113 : return Write_Emit_FMT(type);
440 : }
441 :
442 266859359 : bool AP_Logger_Backend::WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical, bool writev_streaming)
443 : {
444 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !APM_BUILD_TYPE(APM_BUILD_Replay)
445 266859359 : validate_WritePrioritisedBlock(pBuffer, size);
446 : #endif
447 266859325 : if (!ShouldLog(is_critical)) {
448 4480343 : return false;
449 : }
450 262378574 : if (StartNewLogOK()) {
451 95 : start_new_log();
452 : }
453 262378405 : if (!WritesOK()) {
454 482385 : return false;
455 : }
456 :
457 261896117 : if (!is_critical && rate_limiter != nullptr) {
458 228712485 : const uint8_t *msgbuf = (const uint8_t *)pBuffer;
459 228712485 : if (!rate_limiter->should_log(msgbuf[2], writev_streaming)) {
460 162748193 : return false;
461 : }
462 : }
463 :
464 99147732 : if (!ensure_format_emitted(pBuffer, size)) {
465 126 : return false;
466 : }
467 :
468 99148119 : return _WritePrioritisedBlock(pBuffer, size, is_critical);
469 : }
470 :
471 266859320 : bool AP_Logger_Backend::ShouldLog(bool is_critical)
472 : {
473 266859320 : if (!_front.WritesEnabled()) {
474 72802 : return false;
475 : }
476 266786493 : if (!_initialised) {
477 0 : return false;
478 : }
479 :
480 275462926 : if (!_startup_messagewriter->finished() &&
481 8676414 : !hal.scheduler->in_main_thread()) {
482 : // only the main thread may write startup messages out
483 98717 : return false;
484 : }
485 :
486 266694141 : if (_front.in_log_download() &&
487 6332 : _front._last_mavlink_log_transfer_message_handled_ms != 0) {
488 6332 : if (AP_HAL::millis() - _front._last_mavlink_log_transfer_message_handled_ms < 10000) {
489 6332 : if (!_front.vehicle_is_armed()) {
490 : // user is transferring files via mavlink
491 6332 : return false;
492 : }
493 : } else {
494 0 : _front._last_mavlink_log_transfer_message_handled_ms = 0;
495 : }
496 : }
497 :
498 266681403 : if (is_critical && have_logged_armed && !_front._params.file_disarm_rot) {
499 : // if we have previously logged while armed then we log all
500 : // critical messages from then on. That fixes a problem where
501 : // logs show the wrong flight mode if you disarm then arm again
502 1619508 : return true;
503 : }
504 :
505 265061895 : if (!_front.vehicle_is_armed() && !_front.log_while_disarmed()) {
506 4302498 : return false;
507 : }
508 :
509 260759200 : if (_front.vehicle_is_armed()) {
510 192281902 : have_logged_armed = true;
511 : }
512 :
513 260759233 : return true;
514 : }
515 :
516 1032 : void AP_Logger_Backend::PrepForArming()
517 : {
518 1032 : if (_rotate_pending) {
519 36 : _rotate_pending = false;
520 36 : stop_logging();
521 : }
522 1032 : if (logging_started()) {
523 848 : return;
524 : }
525 184 : PrepForArming_start_logging();
526 : }
527 :
528 4037 : bool AP_Logger_Backend::Write_MessageF(const char *fmt, ...)
529 : {
530 4037 : char msg[65] {}; // sizeof(log_Message.msg) + null-termination
531 :
532 : va_list ap;
533 4037 : va_start(ap, fmt);
534 4037 : hal.util->vsnprintf(msg, sizeof(msg), fmt, ap);
535 4037 : va_end(ap);
536 :
537 4037 : return Write_Message(msg);
538 : }
539 :
540 : #if HAL_RALLY_ENABLED
541 : // Write rally points
542 19 : bool AP_Logger_Backend::Write_RallyPoint(uint8_t total,
543 : uint8_t sequence,
544 : const RallyLocation &rally_point)
545 : {
546 19 : const struct log_Rally pkt_rally{
547 : LOG_PACKET_HEADER_INIT(LOG_RALLY_MSG),
548 19 : time_us : AP_HAL::micros64(),
549 : total : total,
550 : sequence : sequence,
551 19 : latitude : rally_point.lat,
552 19 : longitude : rally_point.lng,
553 19 : altitude : rally_point.alt,
554 19 : flags : rally_point.flags
555 19 : };
556 19 : return WriteBlock(&pkt_rally, sizeof(pkt_rally));
557 : }
558 :
559 : // Write rally points
560 886 : bool AP_Logger_Backend::Write_Rally()
561 : {
562 : // kick off asynchronous write:
563 886 : return _startup_messagewriter->writeallrallypoints();
564 : }
565 : #endif
566 :
567 : #if HAL_LOGGER_FENCE_ENABLED
568 : // Write a fence point
569 1843 : bool AP_Logger_Backend::Write_FencePoint(uint8_t total, uint8_t sequence, const AC_PolyFenceItem &fence_point)
570 : {
571 1843 : const struct log_Fence pkt_fence{
572 : LOG_PACKET_HEADER_INIT(LOG_FENCE_MSG),
573 1843 : time_us : AP_HAL::micros64(),
574 : total : total,
575 : sequence : sequence,
576 1843 : type : uint8_t(fence_point.type),
577 1843 : latitude : fence_point.loc.x,
578 1843 : longitude : fence_point.loc.y,
579 1843 : vertex_count : fence_point.vertex_count,
580 1843 : radius : fence_point.radius
581 1843 : };
582 1843 : return WriteBlock(&pkt_fence, sizeof(pkt_fence));
583 : }
584 :
585 : // Write all fence points
586 1001 : bool AP_Logger_Backend::Write_Fence()
587 : {
588 : // kick off asynchronous write:
589 1001 : return _startup_messagewriter->writeallfence();
590 : }
591 : #endif // HAL_LOGGER_FENCE_ENABLED
592 :
593 :
594 2021 : bool AP_Logger_Backend::Write_VER()
595 : {
596 2021 : const AP_FWVersion &fwver = AP::fwversion();
597 :
598 2021 : log_VER pkt{
599 : LOG_PACKET_HEADER_INIT(LOG_VER_MSG),
600 2021 : time_us : AP_HAL::micros64(),
601 2021 : board_type : fwver.board_type,
602 2021 : board_subtype: fwver.board_subtype,
603 2021 : major: fwver.major,
604 2021 : minor: fwver.minor,
605 2021 : patch: fwver.patch,
606 2021 : fw_type: fwver.fw_type,
607 2021 : git_hash: fwver.fw_hash,
608 : #if HAL_WITH_IO_MCU
609 : iomcu_mcu_id : AP::iomcu()->get_mcu_id(),
610 : iomcu_cpu_id : AP::iomcu()->get_cpu_id(),
611 : #else
612 : iomcu_mcu_id : 0,
613 : iomcu_cpu_id : 0,
614 : #endif // HAL_WITH_IO_MCU
615 2021 : };
616 2021 : strncpy(pkt.fw_string, fwver.fw_string, ARRAY_SIZE(pkt.fw_string)-1);
617 :
618 : #ifdef APJ_BOARD_ID
619 : pkt._APJ_BOARD_ID = APJ_BOARD_ID;
620 : #endif
621 2021 : pkt.build_type = fwver.vehicle_type;
622 2021 : pkt.filter_version = AP_FILTER_VERSION;
623 :
624 2021 : return WriteCriticalBlock(&pkt, sizeof(pkt));
625 : }
626 :
627 : /*
628 : convert a list entry number back into a log number (which can then
629 : be converted into a filename). A "list entry number" is a sequence
630 : where the oldest log has a number of 1, the second-from-oldest 2,
631 : and so on. Thus the highest list entry number is equal to the
632 : number of logs.
633 : */
634 405370 : uint16_t AP_Logger_Backend::log_num_from_list_entry(const uint16_t list_entry)
635 : {
636 405370 : uint16_t oldest_log = find_oldest_log();
637 405370 : if (oldest_log == 0) {
638 0 : return 0;
639 : }
640 :
641 405370 : uint32_t log_num = oldest_log + list_entry - 1;
642 405370 : const auto max_logs_num = _front.get_max_num_logs();
643 405370 : if (log_num > (uint32_t)max_logs_num) {
644 4 : log_num -= max_logs_num;
645 : }
646 405370 : return (uint16_t)log_num;
647 : }
648 :
649 : // find_oldest_log - find oldest log
650 : // returns 0 if no log was found
651 73436 : uint16_t AP_Logger_Backend::find_oldest_log()
652 : {
653 73436 : if (_cached_oldest_log != 0) {
654 73430 : return _cached_oldest_log;
655 : }
656 :
657 6 : uint16_t last_log_num = find_last_log();
658 6 : if (last_log_num == 0) {
659 0 : return 0;
660 : }
661 :
662 6 : _cached_oldest_log = last_log_num - get_num_logs() + 1;
663 :
664 6 : return _cached_oldest_log;
665 : }
666 :
667 1131 : void AP_Logger_Backend::vehicle_was_disarmed()
668 : {
669 1131 : if (_front._params.file_disarm_rot &&
670 1131 : !_front._params.log_replay) {
671 : // rotate our log. Closing the current one and letting the
672 : // logging restart naturally based on log_disarmed should do
673 : // the trick:
674 61 : _rotate_pending = true;
675 : }
676 1131 : }
677 :
678 : // we are enabled if we should be logging at the moment
679 416654535 : bool AP_Logger_Backend::logging_enabled() const
680 : {
681 553896655 : if (hal.util->get_soft_armed() ||
682 137242121 : _front.log_while_disarmed()) {
683 398433954 : return true;
684 : }
685 18220580 : return false;
686 : }
687 :
688 150292 : void AP_Logger_Backend::Write_AP_Logger_Stats_File(const struct df_stats &_stats)
689 : {
690 150292 : const struct log_DSF pkt {
691 : LOG_PACKET_HEADER_INIT(LOG_DF_FILE_STATS),
692 150292 : time_us : AP_HAL::micros64(),
693 150292 : dropped : _dropped,
694 150292 : blocks : _stats.blocks,
695 150292 : bytes : _stats.bytes,
696 150292 : buf_space_min : _stats.buf_space_min,
697 150292 : buf_space_max : _stats.buf_space_max,
698 150292 : buf_space_avg : (_stats.blocks) ? (_stats.buf_space_sigma / _stats.blocks) : 0,
699 150292 : };
700 150292 : WriteBlock(&pkt, sizeof(pkt));
701 150292 : }
702 :
703 98944045 : void AP_Logger_Backend::df_stats_gather(const uint16_t bytes_written, uint32_t space_remaining)
704 : {
705 98944045 : if (space_remaining < stats.buf_space_min) {
706 15209158 : stats.buf_space_min = space_remaining;
707 : }
708 98944045 : if (space_remaining > stats.buf_space_max) {
709 451505 : stats.buf_space_max = space_remaining;
710 : }
711 98944045 : stats.buf_space_sigma += space_remaining;
712 98944045 : stats.bytes += bytes_written;
713 98944045 : stats.blocks++;
714 98944045 : }
715 :
716 152385 : void AP_Logger_Backend::df_stats_clear() {
717 152385 : memset(&stats, '\0', sizeof(stats));
718 152385 : stats.buf_space_min = -1;
719 152385 : }
720 :
721 150292 : void AP_Logger_Backend::df_stats_log() {
722 150292 : Write_AP_Logger_Stats_File(stats);
723 150292 : df_stats_clear();
724 150292 : }
725 :
726 :
727 : // class to handle rate limiting of log messages
728 1835 : AP_Logger_RateLimiter::AP_Logger_RateLimiter(const AP_Logger &_front, const AP_Float &_limit_hz, const AP_Float &_disarm_limit_hz)
729 1835 : : front(_front),
730 1835 : rate_limit_hz(_limit_hz),
731 1835 : disarm_rate_limit_hz(_disarm_limit_hz)
732 : {
733 1835 : }
734 :
735 : /*
736 : return false if a streaming message should not be sent yet
737 : */
738 139295872 : bool AP_Logger_RateLimiter::should_log_streaming(uint8_t msgid, float rate_hz)
739 : {
740 139295872 : if (front._log_pause) {
741 0 : return false;
742 : }
743 139295872 : const uint16_t now = AP_HAL::millis16();
744 139295840 : uint16_t delta_ms = now - last_send_ms[msgid];
745 139295840 : if (is_positive(rate_hz) && delta_ms < 1000.0 / rate_hz) {
746 : // too soon
747 97911608 : return false;
748 : }
749 41384297 : last_send_ms[msgid] = now;
750 41384297 : return true;
751 : }
752 :
753 : /*
754 : return true if the message is not a streaming message or the gap
755 : from the last message is more than the message rate
756 : */
757 228712452 : bool AP_Logger_RateLimiter::should_log(uint8_t msgid, bool writev_streaming)
758 : {
759 228712452 : float rate_hz = rate_limit_hz;
760 228712356 : if (!hal.util->get_soft_armed() &&
761 282229151 : !AP::logger().in_log_persistance() &&
762 53516681 : !is_zero(disarm_rate_limit_hz)) {
763 53510846 : rate_hz = disarm_rate_limit_hz;
764 : }
765 228712311 : if (!is_positive(rate_hz) && !front._log_pause) {
766 : // no rate limiting if not paused and rate is zero(user changed the parameter)
767 610915 : return true;
768 : }
769 228101435 : if (last_send_ms[msgid] == 0 && !writev_streaming) {
770 : // might be non streaming. check the not_streaming bitmask
771 : // cache
772 4421661 : if (not_streaming.get(msgid)) {
773 4340504 : return true;
774 : }
775 81155 : const auto *mtype = front.structure_for_msg_type(msgid);
776 81155 : if (mtype == nullptr ||
777 80145 : mtype->streaming == false) {
778 13806 : not_streaming.set(msgid);
779 13806 : return true;
780 : }
781 : }
782 :
783 : #if !defined(HAL_BUILD_AP_PERIPH)
784 : // if we've already decided on sending this msgid in this tick then use the
785 : // same decision again
786 223747123 : const uint16_t sched_ticks = AP::scheduler().ticks();
787 223747949 : if (sched_ticks == last_sched_count[msgid]) {
788 84452039 : return last_return.get(msgid);
789 : }
790 139295910 : last_sched_count[msgid] = sched_ticks;
791 : #endif
792 :
793 139295910 : bool ret = should_log_streaming(msgid, rate_hz);
794 139295838 : if (ret) {
795 41384297 : last_return.set(msgid);
796 : } else {
797 97911541 : last_return.clear(msgid);
798 : }
799 139295831 : return ret;
800 : }
801 :
802 : #endif // HAL_LOGGING_ENABLED
|
