Replay: add option to stop compass after specified time in ms

bugobliterator · Jul 3, 2024 · 23db4a1 · 23db4a1
1 parent 717c5ff
commit 23db4a1
Show file tree

Hide file tree

Showing 6 changed files with 31 additions and 2 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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()