From bdddfae57fa2a6e16b2c4551eff33ccd5136d229 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 4 Jul 2015 22:43:54 +1000
Subject: [PATCH] Replay: don't write out duplicate FMT messages

this makes replay output the same as input for when input is a replay
log
---
 Tools/Replay/DataFlashFileReader.cpp |  7 +++++--
 Tools/Replay/DataFlashFileReader.h   |  8 ++++----
 Tools/Replay/LogReader.cpp           | 27 ++++++++++++++++++++++++++-
 Tools/Replay/LogReader.h             |  9 ++++++---
 Tools/Replay/Replay.cpp              |  7 +++++--
 5 files changed, 46 insertions(+), 12 deletions(-)

diff --git a/Tools/Replay/DataFlashFileReader.cpp b/Tools/Replay/DataFlashFileReader.cpp
index 74d3795329..0bc57c2b95 100644
--- a/Tools/Replay/DataFlashFileReader.cpp
+++ b/Tools/Replay/DataFlashFileReader.cpp
@@ -6,8 +6,6 @@
 #include <stdio.h>
 #include <unistd.h>
 
-DataFlashFileReader::DataFlashFileReader() : fd(-1) {}
-
 bool DataFlashFileReader::open_log(const char *logfile)
 {
     fd = ::open(logfile, O_RDONLY);
@@ -41,6 +39,11 @@ bool DataFlashFileReader::update(char type[5])
         return handle_log_format_msg(f);
     }
 
+    if (!done_format_msgs) {
+        done_format_msgs = true;
+        end_format_msgs();
+    }
+
     const struct log_Format &f = formats[hdr[2]];
     if (f.length == 0) {
         // can't just throw these away as the format specifies the
diff --git a/Tools/Replay/DataFlashFileReader.h b/Tools/Replay/DataFlashFileReader.h
index a7e5df6f33..7f8fe81451 100644
--- a/Tools/Replay/DataFlashFileReader.h
+++ b/Tools/Replay/DataFlashFileReader.h
@@ -6,8 +6,6 @@
 class DataFlashFileReader
 {
 public:
-    DataFlashFileReader();
-
     bool open_log(const char *logfile);
     bool update(char type[5]);
 
@@ -15,10 +13,12 @@ public:
     virtual bool handle_msg(const struct log_Format &f, uint8_t *msg) = 0;
 
 protected:
-    int fd;
+    int fd = -1;
+    bool done_format_msgs = false;
+    virtual void end_format_msgs(void) {}
 
 #define LOGREADER_MAX_FORMATS 255 // must be >= highest MESSAGE
-    struct log_Format formats[LOGREADER_MAX_FORMATS];
+    struct log_Format formats[LOGREADER_MAX_FORMATS] {};
 };
 
 #endif
diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp
index a42e42c0fe..617eee9deb 100644
--- a/Tools/Replay/LogReader.cpp
+++ b/Tools/Replay/LogReader.cpp
@@ -83,7 +83,7 @@ LR_MsgHandler_PARM *parameter_handler;
   messages which we will be generating, so should be discarded
  */
 static const char *generated_names[] = { "EKF1", "EKF2", "EKF3", "EKF4", "EKF5", 
-                                         "AHR2", "POS", NULL };
+                                         "AHR2", "POS", "CHEK", NULL };
 
 /*
   see if a type is in a list of types
@@ -293,3 +293,28 @@ bool LogReader::set_parameter(const char *name, float value)
     }
     return parameter_handler->set_parameter(name, value);
 }
+
+/*
+  called when the last FMT message has been processed
+ */
+void LogReader::end_format_msgs(void)
+{
+    // write out any formats we will be producing
+    for (uint8_t i=0; generated_names[i]; i++) {
+        for (uint8_t n=0; n<num_types; n++) {
+            if (strcmp(generated_names[i], structure[n].name) == 0) {
+                const struct LogStructure *s = &structure[n];
+                struct log_Format pkt {};
+                pkt.head1 = HEAD_BYTE1;
+                pkt.head2 = HEAD_BYTE2;
+                pkt.msgid = LOG_FORMAT_MSG;
+                pkt.type = s->msg_type;
+                pkt.length = s->msg_len;
+                strncpy(pkt.name, s->name, sizeof(pkt.name));
+                strncpy(pkt.format, s->format, sizeof(pkt.format));
+                strncpy(pkt.labels, s->labels, sizeof(pkt.labels));
+                dataflash.WriteBlock(&pkt, sizeof(pkt));
+            }
+        }
+    }
+}
diff --git a/Tools/Replay/LogReader.h b/Tools/Replay/LogReader.h
index 9e1ebf4976..3345e716a2 100644
--- a/Tools/Replay/LogReader.h
+++ b/Tools/Replay/LogReader.h
@@ -27,6 +27,9 @@ public:
     virtual bool handle_log_format_msg(const struct log_Format &f);
     virtual bool handle_msg(const struct log_Format &f, uint8_t *msg);
 
+protected:
+    virtual void end_format_msgs(void) override;
+
 private:
     AP_AHRS &ahrs;
     AP_InertialSensor &ins;
@@ -36,9 +39,6 @@ private:
     AP_Airspeed &airspeed;
     DataFlash_Class &dataflash;
 
-    const struct LogStructure *structure;
-    uint8_t num_types;
-
     uint8_t accel_mask;
     uint8_t gyro_mask;
     bool use_imt = true;
@@ -60,6 +60,9 @@ private:
     // next available msgid for mapping
     uint8_t next_msgid = 1;
 
+    const struct LogStructure *structure;
+    uint8_t num_types;
+
     LR_MsgHandler::CheckState check_state;
 
     bool installed_vehicle_specific_parsers;
diff --git a/Tools/Replay/Replay.cpp b/Tools/Replay/Replay.cpp
index 7f542bfa1a..5ed0b02201 100644
--- a/Tools/Replay/Replay.cpp
+++ b/Tools/Replay/Replay.cpp
@@ -172,8 +172,11 @@ static const struct LogStructure log_structure[] PROGMEM = {
       "CHEK", "QccCLLffff",  "TimeUS,Roll,Pitch,Yaw,Lat,Lng,Alt,VN,VE,VD" }
 };
 
-void ReplayVehicle::setup(void) {
-    dataflash.Init(log_structure, sizeof(log_structure)/sizeof(log_structure[0]));
+void ReplayVehicle::setup(void) 
+{
+    // we pass zero log structures, as we will be outputting the log
+    // structures we need manually, to prevent FMT duplicates
+    dataflash.Init(log_structure, 0);
     dataflash.StartNewLog();
 
     ahrs.set_compass(&compass);