From 23db4a1311dafb18af27f6992b9569a415852836 Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Wed, 3 Jul 2024 10:03:05 +1000
Subject: [PATCH] Replay: add option to stop compass after specified time in ms

---
 Tools/Replay/LR_MsgHandler.cpp |  8 ++++++++
 Tools/Replay/LR_MsgHandler.h   |  7 +++++++
 Tools/Replay/LogReader.cpp     |  2 +-
 Tools/Replay/LogReader.h       |  3 +++
 Tools/Replay/Parameters.h      |  2 ++
 Tools/Replay/Replay.cpp        | 11 ++++++++++-
 6 files changed, 31 insertions(+), 2 deletions(-)

diff --git a/Tools/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp
index cbdca233afaf68..0a9a250277c63f 100644
--- a/Tools/Replay/LR_MsgHandler.cpp
+++ b/Tools/Replay/LR_MsgHandler.cpp
@@ -243,7 +243,15 @@ void LR_MsgHandler_RMGH::process_message(uint8_t *msgbytes)
 
 void LR_MsgHandler_RMGI::process_message(uint8_t *msgbytes)
 {
+    if (compass_data_stopped) {
+        return;
+    }
     MSG_CREATE(RMGI, msgbytes);
+    if (stop_compass_ms != 0 && msg.last_update_usec > (stop_compass_ms * 1000)) {
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Compass data stopped at %u ms\n", stop_compass_ms);
+        compass_data_stopped = true;
+        return;
+    }
     AP::dal().handle_message(msg);
 }
 
diff --git a/Tools/Replay/LR_MsgHandler.h b/Tools/Replay/LR_MsgHandler.h
index 4e30a6571d35ce..c752a4a3bb1b6a 100644
--- a/Tools/Replay/LR_MsgHandler.h
+++ b/Tools/Replay/LR_MsgHandler.h
@@ -217,8 +217,15 @@ class LR_MsgHandler_RMGH : public LR_MsgHandler
 class LR_MsgHandler_RMGI : public LR_MsgHandler
 {
 public:
+    LR_MsgHandler_RMGI(log_Format &_f, uint32_t compass_stop_ms) :
+        LR_MsgHandler(_f),
+        stop_compass_ms(compass_stop_ms)
+        {}
     using LR_MsgHandler::LR_MsgHandler;
     void process_message(uint8_t *msg) override;
+private:
+    uint32_t stop_compass_ms;
+    bool compass_data_stopped;
 };
 class LR_MsgHandler_RBCH : public LR_MsgHandler
 {
diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp
index 7c1b1a43948695..7ff067e067c0da 100644
--- a/Tools/Replay/LogReader.cpp
+++ b/Tools/Replay/LogReader.cpp
@@ -107,7 +107,7 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
 	} else if (streq(name, "RMGH")) {
 	    msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RMGH(formats[f.type]);
 	} else if (streq(name, "RMGI")) {
-	    msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RMGI(formats[f.type]);
+	    msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RMGI(formats[f.type], stop_compass_data_ms);
 	} else if (streq(name, "RBCH")) {
 	    msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RBCH(formats[f.type]);
 	} else if (streq(name, "RBCI")) {
diff --git a/Tools/Replay/LogReader.h b/Tools/Replay/LogReader.h
index 340828c9670b23..0f225be627622d 100644
--- a/Tools/Replay/LogReader.h
+++ b/Tools/Replay/LogReader.h
@@ -20,12 +20,15 @@ class LogReader : public AP_LoggerFileReader
 
     static bool in_list(const char *type, const char *list[]);
 
+    void set_stop_compass_data_ms(uint32_t time_ms) { stop_compass_data_ms = time_ms; }
+
 protected:
 
 private:
 
     NavEKF2 &ekf2;
     NavEKF3 &ekf3;
+    uint32_t stop_compass_data_ms;
 
     struct LogStructure *_log_structure;
     uint8_t _log_structure_count;
diff --git a/Tools/Replay/Parameters.h b/Tools/Replay/Parameters.h
index af17de6997d4f1..aea4cf59a0106a 100644
--- a/Tools/Replay/Parameters.h
+++ b/Tools/Replay/Parameters.h
@@ -17,8 +17,10 @@ class Parameters {
         k_param_logger,
         k_param_NavEKF3,
         k_param_gps,
+        k_param_compass_stop_ms,
     };
     AP_Int8 dummy;
+    AP_Int32 compass_stop_ms;
 };
 
 extern const AP_Param::Info var_info[];
diff --git a/Tools/Replay/Replay.cpp b/Tools/Replay/Replay.cpp
index 2166885e4d5b21..ca87b9cffcf856 100644
--- a/Tools/Replay/Replay.cpp
+++ b/Tools/Replay/Replay.cpp
@@ -76,7 +76,14 @@ const AP_Param::Info ReplayVehicle::var_info[] = {
     // @Group: GPS
     // @Path: ../libraries/AP_GPS/AP_GPS.cpp
     GOBJECT(gps, "GPS", AP_GPS),
-    
+
+    // @Param: COMPASS_DROP_MS
+    // @DisplayName: Time since boot from when to stop Compass data
+    // @Description: Time since boot from when to stop Compass data
+    // @Range: 0 10000
+    // @User: Advanced
+    GSCALAR(compass_stop_ms, "COMPASS_STOP_MS", 0),
+
     AP_VAREND
 };
 
@@ -239,6 +246,8 @@ void Replay::setup()
         ::printf("open(%s): %m\n", filename);
         exit(1);
     }
+    GCS_SEND_TEXT(MAV_SEVERITY_ALERT, "Compass will stop at %u ms", (unsigned int)_vehicle.g.compass_stop_ms);
+    reader.set_stop_compass_data_ms(_vehicle.g.compass_stop_ms);
 }
 
 void Replay::loop()