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

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

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

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

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;

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) {
+void ReplayVehicle::setup(void) 
-    dataflash.Init(log_structure, sizeof(log_structure)/sizeof(log_structure[0]));
+{
+    // 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);