libraries/AP_Logger/AP_Logger_Backend.cpp

   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 AP_Logger_Backend::AP_Logger_Backend(AP_Logger &front,
  25                                      class LoggerMessageWriter_DFLogStart *writer) :
  26     _front(front),
  27     _startup_messagewriter(writer)
  28 {
  29     writer->set_logger_backend(this);
  30 }
  31
  32 uint8_t AP_Logger_Backend::num_types() const
  33 {
  34     return _front._num_types;
  35 }
  36
  37 const struct LogStructure *AP_Logger_Backend::structure(uint8_t num) const
  38 {
  39     return _front.structure(num);
  40 }
  41
  42 uint8_t AP_Logger_Backend::num_units() const
  43 {
  44     return _front._num_units;
  45 }
  46
  47 const struct UnitStructure *AP_Logger_Backend::unit(uint8_t num) const
  48 {
  49     return _front.unit(num);
  50 }
  51
  52 uint8_t AP_Logger_Backend::num_multipliers() const
  53 {
  54     return _front._num_multipliers;
  55 }
  56
  57 const struct MultiplierStructure *AP_Logger_Backend::multiplier(uint8_t num) const
  58 {
  59     return _front.multiplier(num);
  60 }
  61
  62 AP_Logger_Backend::vehicle_startup_message_Writer AP_Logger_Backend::vehicle_message_writer() const {
  63     return _front._vehicle_messages;
  64 }
  65
  66 void AP_Logger_Backend::periodic_10Hz(const uint32_t now)
  67 {
  68 }
  69
  70 void AP_Logger_Backend::periodic_1Hz()
  71 {
  72     if (_rotate_pending && !logging_enabled()) {
  73         _rotate_pending = false;
  74         // handle log rotation once we stop logging
  75         stop_logging_async();
  76     }
  77     df_stats_log();
  78 }
  79
  80 void AP_Logger_Backend::periodic_fullrate()
  81 {
  82     push_log_blocks();
  83 }
  84
  85 void AP_Logger_Backend::periodic_tasks()
  86 {
  87     uint32_t now = AP_HAL::millis();
  88     if (now - _last_periodic_1Hz > 1000) {
  89         periodic_1Hz();
  90         _last_periodic_1Hz = now;
  91     }
  92     if (now - _last_periodic_10Hz > 100) {
  93         periodic_10Hz(now);
  94         _last_periodic_10Hz = now;
  95     }
  96     periodic_fullrate();
  97 }
  98
  99 void AP_Logger_Backend::start_new_log_reset_variables()
 100 {
 101     _dropped = 0;
 102     _startup_messagewriter->reset();
 103     _front.backend_starting_new_log(this);
 104     _formats_written.clearall();
 105 }
 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 bool AP_Logger_Backend::allow_start_ekf() const
 110 {
 111     if (!_startup_messagewriter->fmt_done()) {
 112         return false;
 113     }
 114     // we need to push all startup messages out, or the code in
 115     // WriteBlockCheckStartupMessages bites us.
 116     if (!_startup_messagewriter->finished()) {
 117         return false;
 118     }
 119     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 void AP_Logger_Backend::push_log_blocks() {
 125     WriteMoreStartupMessages();
 126 }
 127
 128 // source more messages from the startup message writer:
 129 void AP_Logger_Backend::WriteMoreStartupMessages()
 130 {
 131 #if APM_BUILD_TYPE(APM_BUILD_Replay)
 132     return;
 133 #endif
 134
 135     if (_startup_messagewriter->finished()) {
 136         return;
 137     }
 138
 139     _writing_startup_messages = true;
 140     _startup_messagewriter->process();
 141     _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 void AP_Logger_Backend::Safe_Write_Emit_FMT(uint8_t msg_type)
 151 {
 152     if (have_emitted_format_for_type(LogMessages(msg_type))) {
 153         return;
 154     }
 155     Write_Emit_FMT(msg_type);
 156 }
 157
 158 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     struct AP_Logger::log_write_fmt_strings ls = {};
 169     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     };
 179     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         INTERNAL_ERROR(AP_InternalError::error_t::logger_missing_logstructure);
 184         return false;
 185     }
 186
 187     if (!Write_Format(&logstruct)) {
 188         return false;
 189     }
 190     if (!Write_Format_Units(&logstruct)) {
 191         return false;
 192     }
 193
 194     return true;
 195 }
 196
 197 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     const char *fmt  = nullptr;
 203     uint8_t msg_len;
 204     AP_Logger::log_write_fmt *f;
 205     for (f = _front.log_write_fmts; f; f=f->next) {
 206         if (f->msg_type == msg_type) {
 207             fmt = f->fmt;
 208             msg_len = f->msg_len;
 209             break;
 210         }
 211     }
 212     if (fmt == nullptr) {
 213         INTERNAL_ERROR(AP_InternalError::error_t::logger_logwrite_missingfmt);
 214         return false;
 215     }
 216     if (bufferspace_available() < msg_len) {
 217         return false;
 218     }
 219
 220     uint8_t buffer[msg_len];
 221     uint8_t offset = 0;
 222     buffer[offset++] = HEAD_BYTE1;
 223     buffer[offset++] = HEAD_BYTE2;
 224     buffer[offset++] = msg_type;
 225     for (uint8_t i=0; i<strlen(fmt); i++) {
 226         uint8_t charlen = 0;
 227         switch(fmt[i]) {
 228         case 'b': {
 229             int8_t tmp = va_arg(arg_list, int);
 230             memcpy(&buffer[offset], &tmp, sizeof(int8_t));
 231             offset += sizeof(int8_t);
 232             break;
 233         }
 234         case 'h':
 235         case 'c': {
 236             int16_t tmp = va_arg(arg_list, int);
 237             memcpy(&buffer[offset], &tmp, sizeof(int16_t));
 238             offset += sizeof(int16_t);
 239             break;
 240         }
 241         case 'd': {
 242             double tmp = va_arg(arg_list, double);
 243             memcpy(&buffer[offset], &tmp, sizeof(double));
 244             offset += sizeof(double);
 245             break;
 246         }
 247         case 'i':
 248         case 'L':
 249         case 'e': {
 250             int32_t tmp = va_arg(arg_list, int);
 251             memcpy(&buffer[offset], &tmp, sizeof(int32_t));
 252             offset += sizeof(int32_t);
 253             break;
 254         }
 255         case 'f': {
 256             float tmp = va_arg(arg_list, double);
 257             memcpy(&buffer[offset], &tmp, sizeof(float));
 258             offset += sizeof(float);
 259             break;
 260         }
 261         case 'g': {
 262             Float16_t tmp;
 263             tmp.set(va_arg(arg_list, double));;
 264             memcpy(&buffer[offset], &tmp, sizeof(tmp));
 265             offset += sizeof(tmp);
 266             break;
 267         }
 268         case 'n':
 269             charlen = 4;
 270             break;
 271         case 'M':
 272         case 'B': {
 273             uint8_t tmp = va_arg(arg_list, int);
 274             memcpy(&buffer[offset], &tmp, sizeof(uint8_t));
 275             offset += sizeof(uint8_t);
 276             break;
 277         }
 278         case 'H':
 279         case 'C': {
 280             uint16_t tmp = va_arg(arg_list, int);
 281             memcpy(&buffer[offset], &tmp, sizeof(uint16_t));
 282             offset += sizeof(uint16_t);
 283             break;
 284         }
 285         case 'I':
 286         case 'E': {
 287             uint32_t tmp = va_arg(arg_list, uint32_t);
 288             memcpy(&buffer[offset], &tmp, sizeof(uint32_t));
 289             offset += sizeof(uint32_t);
 290             break;
 291         }
 292         case 'N':
 293             charlen = 16;
 294             break;
 295         case 'Z':
 296             charlen = 64;
 297             break;
 298         case 'q': {
 299             int64_t tmp = va_arg(arg_list, int64_t);
 300             memcpy(&buffer[offset], &tmp, sizeof(int64_t));
 301             offset += sizeof(int64_t);
 302             break;
 303         }
 304         case 'Q': {
 305             uint64_t tmp = va_arg(arg_list, uint64_t);
 306             memcpy(&buffer[offset], &tmp, sizeof(uint64_t));
 307             offset += sizeof(uint64_t);
 308             break;
 309         }
 310         case 'a': {
 311             int16_t *tmp = va_arg(arg_list, int16_t*);
 312             const uint8_t bytes = 32*2;
 313             memcpy(&buffer[offset], tmp, bytes);
 314             offset += bytes;
 315             break;
 316         }
 317         }
 318         if (charlen != 0) {
 319             char *tmp = va_arg(arg_list, char*);
 320             uint8_t len = strnlen(tmp, charlen);
 321             memcpy(&buffer[offset], tmp, len);
 322             memset(&buffer[offset+len], 0, charlen-len);
 323             offset += charlen;
 324         }
 325     }
 326
 327     return WritePrioritisedBlock(buffer, msg_len, is_critical, is_streaming);
 328 }
 329
 330 bool AP_Logger_Backend::StartNewLogOK() const
 331 {
 332     if (logging_started()) {
 333         return false;
 334     }
 335     if (_front._log_bitmask == 0) {
 336         return false;
 337     }
 338     if (_front.in_log_download()) {
 339         return false;
 340     }
 341     if (!hal.scheduler->in_main_thread()) {
 342         return false;
 343     }
 344     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 bool AP_Logger_Backend::message_type_from_block(const void *pBuffer, uint16_t size, LogMessages &type) const
 350 {
 351     if (size < 3) {
 352         return false;
 353     }
 354     if (((uint8_t*)pBuffer)[0] != HEAD_BYTE1 ||
 355         ((uint8_t*)pBuffer)[1] != HEAD_BYTE2) {
 356         // Not passed a message
 357         INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
 358         return false;
 359     }
 360     type = LogMessages(((uint8_t*)pBuffer)[2]);
 361     return true;
 362 }
 363
 364 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 365 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     if (count > 65534) {
 372         return;
 373     }
 374     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     if (size < 3) {
 380         AP_HAL::panic("Short prioritised block");
 381     }
 382     LogMessages type;
 383     if (!message_type_from_block(pBuffer, size, type)) {
 384         AP_HAL::panic("Not passed a message");
 385     }
 386     uint8_t type_len;
 387     const char *name_src;
 388     const struct LogStructure *s = _front.structure_for_msg_type(type);
 389     if (s == nullptr) {
 390         const struct AP_Logger::log_write_fmt *t = _front.log_write_fmt_for_msg_type(type);
 391         if (t == nullptr) {
 392             AP_HAL::panic("No structure for msg_type=%u", type);
 393         }
 394         type_len = t->msg_len;
 395         name_src = t->name;
 396     } else {
 397         type_len = s->msg_len;
 398         name_src = s->name;
 399     }
 400     if (type_len != size) {
 401         char name[5] = {}; // get a null-terminated string
 402         if (name_src != nullptr) {
 403             memcpy(name, name_src, 4);
 404         } else {
 405             strncpy(name, "?NM?", ARRAY_SIZE(name));
 406         }
 407         AP_HAL::panic("Size mismatch for %u (%s) (expected=%u got=%u)",
 408                       type, name, type_len, size);
 409     }
 410 }
 411 #endif
 412
 413 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     if (!message_type_from_block(pBuffer, size, type)) {
 423         return false;
 424     }
 425     if (have_emitted_format_for_type(type)) {
 426         return true;
 427     }
 428
 429     // make sure the FMT message has gone out!
 430     if (type == LOG_FORMAT_MSG) {
 431         // kind of?  Our caller is just about to emit this....
 432         return true;
 433     }
 434     if (!have_emitted_format_for_type(LOG_FORMAT_MSG) &&
 435         !Write_Emit_FMT(LOG_FORMAT_MSG)) {
 436         return false;
 437     }
 438
 439     return Write_Emit_FMT(type);
 440 }
 441
 442 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     validate_WritePrioritisedBlock(pBuffer, size);
 446 #endif
 447     if (!ShouldLog(is_critical)) {
 448         return false;
 449     }
 450     if (StartNewLogOK()) {
 451         start_new_log();
 452     }
 453     if (!WritesOK()) {
 454         return false;
 455     }
 456
 457     if (!is_critical && rate_limiter != nullptr) {
 458         const uint8_t *msgbuf = (const uint8_t *)pBuffer;
 459         if (!rate_limiter->should_log(msgbuf[2], writev_streaming)) {
 460             return false;
 461         }
 462     }
 463
 464     if (!ensure_format_emitted(pBuffer, size)) {
 465         return false;
 466     }
 467
 468     return _WritePrioritisedBlock(pBuffer, size, is_critical);
 469 }
 470
 471 bool AP_Logger_Backend::ShouldLog(bool is_critical)
 472 {
 473     if (!_front.WritesEnabled()) {
 474         return false;
 475     }
 476     if (!_initialised) {
 477         return false;
 478     }
 479
 480     if (!_startup_messagewriter->finished() &&
 481         !hal.scheduler->in_main_thread()) {
 482         // only the main thread may write startup messages out
 483         return false;
 484     }
 485
 486     if (_front.in_log_download() &&
 487         _front._last_mavlink_log_transfer_message_handled_ms != 0) {
 488         if (AP_HAL::millis() - _front._last_mavlink_log_transfer_message_handled_ms < 10000) {
 489             if (!_front.vehicle_is_armed()) {
 490                 // user is transferring files via mavlink
 491                 return false;
 492             }
 493         } else {
 494             _front._last_mavlink_log_transfer_message_handled_ms = 0;
 495         }
 496     }
 497
 498     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         return true;
 503     }
 504
 505     if (!_front.vehicle_is_armed() && !_front.log_while_disarmed()) {
 506         return false;
 507     }
 508
 509     if (_front.vehicle_is_armed()) {
 510         have_logged_armed = true;
 511     }
 512
 513     return true;
 514 }
 515
 516 void AP_Logger_Backend::PrepForArming()
 517 {
 518     if (_rotate_pending) {
 519         _rotate_pending = false;
 520         stop_logging();
 521     }
 522     if (logging_started()) {
 523         return;
 524     }
 525     PrepForArming_start_logging();
 526 }
 527
 528 bool AP_Logger_Backend::Write_MessageF(const char *fmt, ...)
 529 {
 530     char msg[65] {}; // sizeof(log_Message.msg) + null-termination
 531
 532     va_list ap;
 533     va_start(ap, fmt);
 534     hal.util->vsnprintf(msg, sizeof(msg), fmt, ap);
 535     va_end(ap);
 536
 537     return Write_Message(msg);
 538 }
 539
 540 #if HAL_RALLY_ENABLED
 541 // Write rally points
 542 bool AP_Logger_Backend::Write_RallyPoint(uint8_t total,
 543                                          uint8_t sequence,
 544                                          const RallyLocation &rally_point)
 545 {
 546     const struct log_Rally pkt_rally{
 547         LOG_PACKET_HEADER_INIT(LOG_RALLY_MSG),
 548         time_us         : AP_HAL::micros64(),
 549         total           : total,
 550         sequence        : sequence,
 551         latitude        : rally_point.lat,
 552         longitude       : rally_point.lng,
 553         altitude        : rally_point.alt,
 554         flags           : rally_point.flags
 555     };
 556     return WriteBlock(&pkt_rally, sizeof(pkt_rally));
 557 }
 558
 559 // Write rally points
 560 bool AP_Logger_Backend::Write_Rally()
 561 {
 562     // kick off asynchronous write:
 563     return _startup_messagewriter->writeallrallypoints();
 564 }
 565 #endif
 566
 567 #if HAL_LOGGER_FENCE_ENABLED
 568 // Write a fence point
 569 bool AP_Logger_Backend::Write_FencePoint(uint8_t total, uint8_t sequence, const AC_PolyFenceItem &fence_point)
 570 {
 571     const struct log_Fence pkt_fence{
 572         LOG_PACKET_HEADER_INIT(LOG_FENCE_MSG),
 573         time_us         : AP_HAL::micros64(),
 574         total           : total,
 575         sequence        : sequence,
 576         type            : uint8_t(fence_point.type),
 577         latitude        : fence_point.loc.x,
 578         longitude       : fence_point.loc.y,
 579         vertex_count    : fence_point.vertex_count,
 580         radius          : fence_point.radius
 581     };
 582     return WriteBlock(&pkt_fence, sizeof(pkt_fence));
 583 }
 584
 585 // Write all fence points
 586 bool AP_Logger_Backend::Write_Fence()
 587 {
 588     // kick off asynchronous write:
 589     return _startup_messagewriter->writeallfence();
 590 }
 591 #endif // HAL_LOGGER_FENCE_ENABLED
 592
 593
 594 bool AP_Logger_Backend::Write_VER()
 595 {
 596     const AP_FWVersion &fwver = AP::fwversion();
 597
 598     log_VER pkt{
 599         LOG_PACKET_HEADER_INIT(LOG_VER_MSG),
 600         time_us  : AP_HAL::micros64(),
 601         board_type : fwver.board_type,
 602         board_subtype: fwver.board_subtype,
 603         major: fwver.major,
 604         minor: fwver.minor,
 605         patch: fwver.patch,
 606         fw_type: fwver.fw_type,
 607         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     };
 616     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     pkt.build_type = fwver.vehicle_type;
 622     pkt.filter_version = AP_FILTER_VERSION;
 623
 624     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 uint16_t AP_Logger_Backend::log_num_from_list_entry(const uint16_t list_entry)
 635 {
 636     uint16_t oldest_log = find_oldest_log();
 637     if (oldest_log == 0) {
 638         return 0;
 639     }
 640
 641     uint32_t log_num = oldest_log + list_entry - 1;
 642     const auto max_logs_num = _front.get_max_num_logs();
 643     if (log_num > (uint32_t)max_logs_num) {
 644         log_num -= max_logs_num;
 645     }
 646     return (uint16_t)log_num;
 647 }
 648
 649 // find_oldest_log - find oldest log
 650 // returns 0 if no log was found
 651 uint16_t AP_Logger_Backend::find_oldest_log()
 652 {
 653     if (_cached_oldest_log != 0) {
 654         return _cached_oldest_log;
 655     }
 656
 657     uint16_t last_log_num = find_last_log();
 658     if (last_log_num == 0) {
 659         return 0;
 660     }
 661
 662     _cached_oldest_log = last_log_num - get_num_logs() + 1;
 663
 664     return _cached_oldest_log;
 665 }
 666
 667 void AP_Logger_Backend::vehicle_was_disarmed()
 668 {
 669     if (_front._params.file_disarm_rot &&
 670         !_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         _rotate_pending = true;
 675     }
 676 }
 677
 678 // this sensor is enabled if we should be logging at the moment
 679 bool AP_Logger_Backend::logging_enabled() const
 680 {
 681     if (hal.util->get_soft_armed() ||
 682         _front.log_while_disarmed()) {
 683         return true;
 684     }
 685     return false;
 686 }
 687
 688 void AP_Logger_Backend::Write_AP_Logger_Stats_File(const struct df_stats &_stats)
 689 {
 690     const struct log_DSF pkt {
 691         LOG_PACKET_HEADER_INIT(LOG_DF_FILE_STATS),
 692         time_us         : AP_HAL::micros64(),
 693         dropped         : _dropped,
 694         blocks          : _stats.blocks,
 695         bytes           : _stats.bytes,
 696         buf_space_min   : _stats.buf_space_min,
 697         buf_space_max   : _stats.buf_space_max,
 698         buf_space_avg   : (_stats.blocks) ? (_stats.buf_space_sigma / _stats.blocks) : 0,
 699     };
 700     WriteBlock(&pkt, sizeof(pkt));
 701 }
 702
 703 void AP_Logger_Backend::df_stats_gather(const uint16_t bytes_written, uint32_t space_remaining)
 704 {
 705     if (space_remaining < stats.buf_space_min) {
 706         stats.buf_space_min = space_remaining;
 707     }
 708     if (space_remaining > stats.buf_space_max) {
 709         stats.buf_space_max = space_remaining;
 710     }
 711     stats.buf_space_sigma += space_remaining;
 712     stats.bytes += bytes_written;
 713     stats.blocks++;
 714 }
 715
 716 void AP_Logger_Backend::df_stats_clear() {
 717     memset(&stats, '\0', sizeof(stats));
 718     stats.buf_space_min = -1;
 719 }
 720
 721 void AP_Logger_Backend::df_stats_log() {
 722     Write_AP_Logger_Stats_File(stats);
 723     df_stats_clear();
 724 }
 725
 726
 727 // class to handle rate limiting of log messages
 728 AP_Logger_RateLimiter::AP_Logger_RateLimiter(const AP_Logger &_front, const AP_Float &_limit_hz, const AP_Float &_disarm_limit_hz)
 729     : front(_front),
 730       rate_limit_hz(_limit_hz),
 731       disarm_rate_limit_hz(_disarm_limit_hz)
 732 {
 733 }
 734
 735 /*
 736   return false if a streaming message should not be sent yet
 737  */
 738 bool AP_Logger_RateLimiter::should_log_streaming(uint8_t msgid, float rate_hz)
 739 {
 740     if (front._log_pause) {
 741         return false;
 742     }
 743     const uint16_t now = AP_HAL::millis16();
 744     uint16_t delta_ms = now - last_send_ms[msgid];
 745     if (is_positive(rate_hz) && delta_ms < 1000.0 / rate_hz) {
 746         // too soon
 747         return false;
 748     }
 749     last_send_ms[msgid] = now;
 750     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 bool AP_Logger_RateLimiter::should_log(uint8_t msgid, bool writev_streaming)
 758 {
 759     float rate_hz = rate_limit_hz;
 760     if (!hal.util->get_soft_armed() &&
 761         !AP::logger().in_log_persistance() &&
 762         !is_zero(disarm_rate_limit_hz)) {
 763         rate_hz = disarm_rate_limit_hz;
 764     }
 765     if (!is_positive(rate_hz) && !front._log_pause) {
 766         // no rate limiting if not paused and rate is zero(user changed the parameter)
 767         return true;
 768     }
 769     if (last_send_ms[msgid] == 0 && !writev_streaming) {
 770         // might be non streaming. check the not_streaming bitmask
 771         // cache
 772         if (not_streaming.get(msgid)) {
 773             return true;
 774         }
 775         const auto *mtype = front.structure_for_msg_type(msgid);
 776         if (mtype == nullptr ||
 777             mtype->streaming == false) {
 778             not_streaming.set(msgid);
 779             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     const uint16_t sched_ticks = AP::scheduler().ticks();
 787     if (sched_ticks == last_sched_count[msgid]) {
 788         return last_return.get(msgid);
 789     }
 790     last_sched_count[msgid] = sched_ticks;
 791 #endif
 792
 793     bool ret = should_log_streaming(msgid, rate_hz);
 794     if (ret) {
 795         last_return.set(msgid);
 796     } else {
 797         last_return.clear(msgid);
 798     }
 799     return ret;
 800 }
 801
 802 #endif  // HAL_LOGGING_ENABLED