AP_Logger: write formats out as required rather than at start of log

libraries/AP_Logger/AP_Logger_Backend.cpp

@@ -101,6 +101,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
@@ -124,42 +125,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()
 {
@@ -363,6 +328,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)
@@ -381,11 +363,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);
@@ -413,6 +394,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)
@@ -435,6 +467,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);
 }
 

libraries/AP_Logger/AP_Logger_Backend.h

@@ -9,6 +9,7 @@
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include <AP_Mission/AP_Mission.h>
 #include <AP_Vehicle/ModeReason.h>
+#include "LogStructure.h"
 
 class LoggerMessageWriter_DFLogStart;
 
@@ -132,6 +133,9 @@ public:
     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 @@ protected:
     /*
       read a block
     */
-    virtual bool WriteBlockCheckStartupMessages();
     virtual void WriteMoreStartupMessages();
     virtual void push_log_blocks();
 
@@ -262,6 +265,12 @@ private:
 
     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

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();

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");

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...

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;
 }
 
 /*

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++;