From c02aef8a503393ff277eade4f067656468c9c2f1 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 6 Mar 2021 17:36:57 +1100
Subject: [PATCH] AP_Logger: allow for new log msgs in Replay

this makes replay tuning and code testing much easier as up to 10 new
log messages can be added in the replay code
---
 libraries/AP_Logger/AP_Logger.cpp         | 17 ++++++++++-------
 libraries/AP_Logger/AP_Logger.h           |  6 ++++++
 libraries/AP_Logger/AP_Logger_Backend.cpp |  6 ++++--
 3 files changed, 20 insertions(+), 9 deletions(-)

diff --git a/libraries/AP_Logger/AP_Logger.cpp b/libraries/AP_Logger/AP_Logger.cpp
index 7ff623f50f..c30002eb19 100644
--- a/libraries/AP_Logger/AP_Logger.cpp
+++ b/libraries/AP_Logger/AP_Logger.cpp
@@ -57,6 +57,14 @@ extern const AP_HAL::HAL& hal;
 # endif
 #endif
 
+// when adding new msgs we start at a different index in replay
+#if APM_BUILD_TYPE(APM_BUILD_Replay)
+#define LOGGING_FIRST_DYNAMIC_MSGID REPLAY_LOG_NEW_MSG_MAX
+#else
+#define LOGGING_FIRST_DYNAMIC_MSGID 254
+#endif
+
+
 const AP_Param::GroupInfo AP_Logger::var_info[] = {
     // @Param: _BACKEND_TYPE
     // @DisplayName: AP_Logger Backend Storage type
@@ -1078,12 +1086,6 @@ AP_Logger::log_write_fmt *AP_Logger::msg_fmt_for_name(const char *name, const ch
         }
     }
 
-#if APM_BUILD_TYPE(APM_BUILD_Replay)
-    // don't allow for new msg types during replay. We will be able to
-    // support these eventually, but for now they cause corruption
-    return nullptr;
-#endif
-
     f = (struct log_write_fmt *)calloc(1, sizeof(*f));
     if (f == nullptr) {
         // out of memory
@@ -1198,8 +1200,9 @@ bool AP_Logger::msg_type_in_use(const uint8_t msg_type) const
 // find a free message type
 int16_t AP_Logger::find_free_msg_type() const
 {
+    const uint8_t start = LOGGING_FIRST_DYNAMIC_MSGID;
     // avoid using 255 here; perhaps we want to use it to extend things later
-    for (uint16_t msg_type=254; msg_type>0; msg_type--) { // more likely to be free at end
+    for (uint16_t msg_type=start; msg_type>0; msg_type--) { // more likely to be free at end
         if (! msg_type_in_use(msg_type)) {
             return msg_type;
         }
diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h
index 5455f71ce7..b23e0ae191 100644
--- a/libraries/AP_Logger/AP_Logger.h
+++ b/libraries/AP_Logger/AP_Logger.h
@@ -48,6 +48,12 @@
 #define HAL_LOGGER_FILE_CONTENTS_ENABLED HAL_LOGGING_FILESYSTEM_ENABLED
 #endif
 
+// range of IDs to allow for new messages during replay. It is very
+// useful to be able to add new messages during a replay, but we need
+// to avoid colliding with existing messages
+#define REPLAY_LOG_NEW_MSG_MAX 230
+#define REPLAY_LOG_NEW_MSG_MIN 220
+
 #include <AC_PID/AC_PID.h>
 #include <AP_HAL/AP_HAL.h>
 #include <AP_AHRS/AP_AHRS.h>
diff --git a/libraries/AP_Logger/AP_Logger_Backend.cpp b/libraries/AP_Logger/AP_Logger_Backend.cpp
index b1a993164d..1ff22835ce 100644
--- a/libraries/AP_Logger/AP_Logger_Backend.cpp
+++ b/libraries/AP_Logger/AP_Logger_Backend.cpp
@@ -172,8 +172,10 @@ void AP_Logger_Backend::WriteMoreStartupMessages()
 bool AP_Logger_Backend::Write_Emit_FMT(uint8_t msg_type)
 {
 #if APM_BUILD_TYPE(APM_BUILD_Replay)
-    // sure, sure we did....
-    return true;
+    if (msg_type < REPLAY_LOG_NEW_MSG_MIN || msg_type > REPLAY_LOG_NEW_MSG_MAX) {
+        // don't re-emit FMU msgs unless they are in the replay range
+        return true;
+    }
 #endif
 
     // get log structure from front end: