diff --git a/libraries/AP_Logger/AP_Logger_Backend.cpp b/libraries/AP_Logger/AP_Logger_Backend.cpp index 8d45d8187e..bf1a544751 100644 --- a/libraries/AP_Logger/AP_Logger_Backend.cpp +++ b/libraries/AP_Logger/AP_Logger_Backend.cpp @@ -100,6 +100,7 @@ void AP_Logger_Backend::start_new_log_reset_variables() _startup_messagewriter->reset(); _front.backend_starting_new_log(this); _log_file_size_bytes = 0; + _formats_written.clearall(); } // We may need to make sure data is loggable before starting the @@ -123,42 +124,6 @@ void AP_Logger_Backend::push_log_blocks() { WriteMoreStartupMessages(); } -// returns true if all format messages have been written, and thus it is OK -// for other messages to go out to the log -bool AP_Logger_Backend::WriteBlockCheckStartupMessages() -{ -#if APM_BUILD_TYPE(APM_BUILD_Replay) - return true; -#endif - - if (_startup_messagewriter->fmt_done()) { - return true; - } - - if (_writing_startup_messages) { - // we have been called by a messagewriter, so writing is OK - return true; - } - - if (!_startup_messagewriter->finished() && - !hal.scheduler->in_main_thread()) { - // only the main thread may write startup messages out - return false; - } - - // we're not writing startup messages, so this must be some random - // caller hoping to write blocks out. Push out log blocks - we - // might end up clearing the buffer..... - push_log_blocks(); - - // even if we did finish writing startup messages, we can't - // permit any message to go in as its timestamp will be before - // any we wrote in. Time going backwards annoys log readers. - - // sorry! currently busy writing out startup messages... - return false; -} - // source more messages from the startup message writer: void AP_Logger_Backend::WriteMoreStartupMessages() { @@ -362,6 +327,23 @@ bool AP_Logger_Backend::StartNewLogOK() const return true; } +// validate that pBuffer looks like a message, extract message type. +// Returns false if this doesn't look like a valid message. +bool AP_Logger_Backend::message_type_from_block(const void *pBuffer, uint16_t size, LogMessages &type) const +{ + if (size < 3) { + return false; + } + if (((uint8_t*)pBuffer)[0] != HEAD_BYTE1 || + ((uint8_t*)pBuffer)[1] != HEAD_BYTE2) { + // Not passed a message + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return false; + } + type = LogMessages(((uint8_t*)pBuffer)[2]); + return true; +} + #if CONFIG_HAL_BOARD == HAL_BOARD_SITL void AP_Logger_Backend::validate_WritePrioritisedBlock(const void *pBuffer, uint16_t size) @@ -380,11 +362,10 @@ void AP_Logger_Backend::validate_WritePrioritisedBlock(const void *pBuffer, if (size < 3) { AP_HAL::panic("Short prioritised block"); } - if (((uint8_t*)pBuffer)[0] != HEAD_BYTE1 || - ((uint8_t*)pBuffer)[1] != HEAD_BYTE2) { + LogMessages type; + if (!message_type_from_block(pBuffer, size, type)) { AP_HAL::panic("Not passed a message"); } - const uint8_t type = ((uint8_t*)pBuffer)[2]; uint8_t type_len; const char *name_src; const struct LogStructure *s = _front.structure_for_msg_type(type); @@ -412,6 +393,57 @@ void AP_Logger_Backend::validate_WritePrioritisedBlock(const void *pBuffer, } #endif +bool AP_Logger_Backend::emit_format_for_type(LogMessages a_type) +{ + // linearly scan the formats structure to find the format for the type: + for (uint8_t i=0; i< num_types(); i++) { + const auto &s { structure(i) }; + if (s == nullptr) { + continue; + } + if (s->msg_type != a_type) { + continue; + } + // found the relevant structure. Attempt to write it: + if (!Write_Format(s)) { + return false; + } + return true; + } + // didn't find the structure. That's probably bad... + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return false; +} + +bool AP_Logger_Backend::ensure_format_emitted(const void *pBuffer, uint16_t size) +{ +#if APM_BUILD_TYPE(APM_BUILD_Replay) + // we trust that Replay will correctly emit formats as required + return true; +#endif + + // extract the ID: + LogMessages type; + if (!message_type_from_block(pBuffer, size, type)) { + return false; + } + if (have_emitted_format_for_type(type)) { + return true; + } + + // make sure the FMT message has gone out! + if (type == LOG_FORMAT_MSG) { + // kind of? Our caller is just about to emit this.... + return true; + } + if (!have_emitted_format_for_type(LOG_FORMAT_MSG) && + !emit_format_for_type(LOG_FORMAT_MSG)) { + return false; + } + + return emit_format_for_type(type); +} + bool AP_Logger_Backend::WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical, bool writev_streaming) { #if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !APM_BUILD_TYPE(APM_BUILD_Replay) @@ -434,6 +466,10 @@ bool AP_Logger_Backend::WritePrioritisedBlock(const void *pBuffer, uint16_t size } } + if (!ensure_format_emitted(pBuffer, size)) { + return false; + } + return _WritePrioritisedBlock(pBuffer, size, is_critical); } diff --git a/libraries/AP_Logger/AP_Logger_Backend.h b/libraries/AP_Logger/AP_Logger_Backend.h index 616ce36b66..e9f99e8aab 100644 --- a/libraries/AP_Logger/AP_Logger_Backend.h +++ b/libraries/AP_Logger/AP_Logger_Backend.h @@ -9,6 +9,7 @@ #include #include #include +#include "LogStructure.h" class LoggerMessageWriter_DFLogStart; @@ -132,6 +133,9 @@ class AP_Logger_Backend bool Write_Fence(); #endif bool Write_Format(const struct LogStructure *structure); + bool have_emitted_format_for_type(LogMessages a_type) const { + return _formats_written.get(uint8_t(a_type)); + } bool Write_Message(const char *message); bool Write_MessageF(const char *fmt, ...); bool Write_Mission_Cmd(const AP_Mission &mission, @@ -195,7 +199,6 @@ class AP_Logger_Backend /* read a block */ - virtual bool WriteBlockCheckStartupMessages(); virtual void WriteMoreStartupMessages(); virtual void push_log_blocks(); @@ -262,6 +265,12 @@ class AP_Logger_Backend void Write_AP_Logger_Stats_File(const struct df_stats &_stats); void validate_WritePrioritisedBlock(const void *pBuffer, uint16_t size); + + bool message_type_from_block(const void *pBuffer, uint16_t size, LogMessages &type) const; + bool ensure_format_emitted(const void *pBuffer, uint16_t size); + bool emit_format_for_type(LogMessages a_type); + Bitmask<256> _formats_written; + }; #endif // HAL_LOGGING_ENABLED diff --git a/libraries/AP_Logger/AP_Logger_Block.cpp b/libraries/AP_Logger/AP_Logger_Block.cpp index f766a66693..5f5343f085 100644 --- a/libraries/AP_Logger/AP_Logger_Block.cpp +++ b/libraries/AP_Logger/AP_Logger_Block.cpp @@ -132,11 +132,6 @@ bool AP_Logger_Block::_WritePrioritisedBlock(const void *pBuffer, uint16_t size, return false; } - if (!WriteBlockCheckStartupMessages()) { - _dropped++; - return false; - } - WITH_SEMAPHORE(write_sem); const uint32_t space = writebuf.space(); diff --git a/libraries/AP_Logger/AP_Logger_File.cpp b/libraries/AP_Logger/AP_Logger_File.cpp index 3208f8cf6e..2826054298 100644 --- a/libraries/AP_Logger/AP_Logger_File.cpp +++ b/libraries/AP_Logger/AP_Logger_File.cpp @@ -474,11 +474,6 @@ bool AP_Logger_File::_WritePrioritisedBlock(const void *pBuffer, uint16_t size, { WITH_SEMAPHORE(semaphore); - if (! WriteBlockCheckStartupMessages()) { - _dropped++; - return false; - } - #if APM_BUILD_TYPE(APM_BUILD_Replay) if (AP::FS().write(_write_fd, pBuffer, size) != size) { AP_HAL::panic("Short write"); diff --git a/libraries/AP_Logger/AP_Logger_MAVLink.cpp b/libraries/AP_Logger/AP_Logger_MAVLink.cpp index cb5ee787f9..1404532014 100644 --- a/libraries/AP_Logger/AP_Logger_MAVLink.cpp +++ b/libraries/AP_Logger/AP_Logger_MAVLink.cpp @@ -136,11 +136,6 @@ bool AP_Logger_MAVLink::_WritePrioritisedBlock(const void *pBuffer, uint16_t siz return false; } - if (! WriteBlockCheckStartupMessages()) { - semaphore.give(); - return false; - } - if (bufferspace_available() < size) { if (_startup_messagewriter->finished()) { // do not count the startup packets as being dropped... diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 2febb3577f..3be0b0cb92 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -60,7 +60,11 @@ bool AP_Logger_Backend::Write_Format(const struct LogStructure *s) { struct log_Format pkt; Fill_Format(s, pkt); - return WriteCriticalBlock(&pkt, sizeof(pkt)); + if (!WriteCriticalBlock(&pkt, sizeof(pkt))) { + return false; + } + _formats_written.set(s->msg_type); + return true; } /* diff --git a/libraries/AP_Logger/LoggerMessageWriter.cpp b/libraries/AP_Logger/LoggerMessageWriter.cpp index 3918fc6226..6d6b2fee80 100644 --- a/libraries/AP_Logger/LoggerMessageWriter.cpp +++ b/libraries/AP_Logger/LoggerMessageWriter.cpp @@ -109,7 +109,12 @@ void LoggerMessageWriter_DFLogStart::process() case Stage::FORMATS: // write log formats so the log is self-describing while (next_format_to_send < _logger_backend->num_types()) { - if (!_logger_backend->Write_Format(_logger_backend->structure(next_format_to_send))) { + const auto &s { _logger_backend->structure(next_format_to_send) }; + if (_logger_backend->have_emitted_format_for_type((LogMessages)s->msg_type)) { + next_format_to_send++; + continue; + } + if (!_logger_backend->Write_Format(s)) { return; // call me again! } next_format_to_send++;