From 5452730fc95f3765ebacc36270690b6998ee1ca7 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Sun, 22 Apr 2018 20:19:11 +1000
Subject: [PATCH] Tools: Replay: make it work again

Tools: Replay: ignore setting of LOG_DISARMED

Otherwise log files that come in with LOG_DISARMED false don't get any
significant output

Tools: Replay: apply user parameters after any PARM message

Tools: Replay: emit timestamp when EKF is force-started

Tools: Replay: use stderr for what it's good for

Tools: Replay: force log disarmed
---
 Tools/Replay/DataFlashFileReader.cpp |   5 -
 Tools/Replay/DataFlashFileReader.h   |   2 -
 Tools/Replay/LR_MsgHandler.cpp       |   3 +-
 Tools/Replay/LogReader.cpp           | 226 ++++++++++++++++++++-------
 Tools/Replay/LogReader.h             |  14 +-
 Tools/Replay/Replay.cpp              |  60 ++++---
 Tools/Replay/Replay.h                |  16 +-
 7 files changed, 229 insertions(+), 97 deletions(-)

diff --git a/Tools/Replay/DataFlashFileReader.cpp b/Tools/Replay/DataFlashFileReader.cpp
index 697157dc35..3310100af2 100644
--- a/Tools/Replay/DataFlashFileReader.cpp
+++ b/Tools/Replay/DataFlashFileReader.cpp
@@ -88,11 +88,6 @@ 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 9ae0302143..c9f4267cd9 100644
--- a/Tools/Replay/DataFlashFileReader.h
+++ b/Tools/Replay/DataFlashFileReader.h
@@ -22,8 +22,6 @@ public:
 
 protected:
     int fd = -1;
-    bool done_format_msgs = false;
-    virtual void end_format_msgs(void) {}
 
     struct log_Format formats[LOGREADER_MAX_FORMATS] {};
 
diff --git a/Tools/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp
index d401cda523..b9bc1650f3 100644
--- a/Tools/Replay/LR_MsgHandler.cpp
+++ b/Tools/Replay/LR_MsgHandler.cpp
@@ -383,7 +383,8 @@ bool LR_MsgHandler_PARM::set_parameter(const char *name, const float value)
 {
     const char *ignore_parms[] = { "GPS_TYPE", "AHRS_EKF_TYPE", "EK2_ENABLE", "EK3_ENABLE"
                                    "COMPASS_ORIENT", "COMPASS_ORIENT2",
-                                   "COMPASS_ORIENT3", "LOG_FILE_BUFSIZE"};
+                                   "COMPASS_ORIENT3", "LOG_FILE_BUFSIZE",
+                                   "LOG_DISARMED"};
     for (uint8_t i=0; i < ARRAY_SIZE(ignore_parms); i++) {
         if (strncmp(name, ignore_parms[i], AP_MAX_NAME_SIZE) == 0) {
             ::printf("Ignoring set of %s to %f\n", name, value);
diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp
index b364b0a482..7bd829ce80 100644
--- a/Tools/Replay/LogReader.cpp
+++ b/Tools/Replay/LogReader.cpp
@@ -18,7 +18,7 @@
 #include "MsgHandler.h"
 #include "Replay.h"
 
-#define DEBUG 0
+#define DEBUG 1
 #if DEBUG
 # define debug(fmt, args...)     printf(fmt "\n", ##args)
 #else
@@ -29,12 +29,19 @@
 
 extern const AP_HAL::HAL& hal;
 
-const struct LogStructure log_structure[] = {
+const struct LogStructure running_codes_log_structure[] = {
     LOG_COMMON_STRUCTURES,
 };
 
-LogReader::LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, Compass &_compass, AP_GPS &_gps, 
-                     AP_Airspeed &_airspeed, DataFlash_Class &_dataflash, const char **&_nottypes):
+LogReader::LogReader(AP_AHRS &_ahrs,
+                     AP_InertialSensor &_ins,
+                     Compass &_compass,
+                     AP_GPS &_gps,
+                     AP_Airspeed &_airspeed,
+                     DataFlash_Class &_dataflash,
+                     struct LogStructure *log_structure,
+                     uint8_t log_structure_count,
+                     const char **&_nottypes):
     DataFlashFileReader(),
     vehicle(VehicleType::VEHICLE_UNKNOWN),
     ahrs(_ahrs),
@@ -47,8 +54,15 @@ LogReader::LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, Compass &_compass,
     gyro_mask(7),
     last_timestamp_usec(0),
     installed_vehicle_specific_parsers(false),
+    _log_structure(log_structure),
     nottypes(_nottypes)
 {
+    if (log_structure_count != 0) {
+        ::fprintf(stderr, "Do NOT put anything in the log_structure before passing it in here");
+        abort(); // so there.
+    }
+
+    initialise_fmt_map();
 }
 
 struct log_Format deferred_formats[LOGREADER_MAX_FORMATS];
@@ -83,17 +97,53 @@ void LogReader::maybe_install_vehicle_specific_parsers() {
 }
 
 /*
-  messages which we will be generating, so should be discarded
+  messages which we will be generating, so should be discarded.
+  Additionally, FMT messages for messages NOT in this list will be
+  passed straight through to the output file, whereas FMT messages for
+  messages IN this list must come from LOG_BASE_STRUCTURES in
+  LogStructure.h
+
+  Note that there is an existing with FMTU messages, as these are
+  emitted both from the common structures at log startup and also when
+  Log_Write(...) is called for the first time for a particular format
+  name.  Since we include it in generated_names FMTU messages will not
+  be passed through from the source logs - but since the Replay code
+  does call Log_Write(...) you will end up with a small selection of
+  FMTU messages from that.
+
  */
-static const char *generated_names[] = { "EKF1", "EKF2", "EKF3", "EKF4", "EKF5",
-                                         "NKF1", "NKF2", "NKF3", "NKF4", "NKF5",
-                                         "NKF6", "NKF7", "NKF8", "NKF9", "NKF10",
-                                         "AHR2", "POS", "CHEK",
-                                         "IMT", "IMT2",
-                                         "MAG", "MAG2",
-                                         "BARO", "BAR2",
-                                         "GPS","GPA",
-                                         "NKA", "NKV", NULL };
+static const char *generated_names[] = {
+    "FMT",
+    "FMTU",
+    "NKF1", "NKF2", "NKF3", "NKF4", "NKF5", "NKF6", "NKF7", "NKF8", "NKF9", "NKF0",
+    "NKQ1", "NKQ2",
+    "XKF1", "XKF2", "XKF3", "XKF4", "XKF5", "XKF6", "XKF7", "XKF8", "XKF9", "XKF0",
+    "XKQ1", "XKQ2", "XKFD", "XKV1", "XKV2",
+    "AHR2",
+    "ORGN",
+    "POS",
+    "CHEK",
+    "IMT", "IMT2", "IMT3",
+    "MAG", "MAG2",
+    "BARO", "BAR2",
+    "GPS","GPA",
+    NULL,
+};
+
+// these names we emit from the code as normal, but are emitted using
+// Log_Write.  Thus they are not present in LOG_COMMON_STRUCTURES.  A
+// format will be written for this by the code itself the first time
+// the message is emitted to the log.  However, we must not write the
+// messages from the old log to the new log, so we need to keep a map
+// of IDs to prune out...
+static const char *log_write_names[] = {
+    "NKA",
+    "NKV",
+
+    "NKT1",
+    "NKT2",
+    nullptr
+};
 
 /*
   see if a type is in a list of types
@@ -111,30 +161,78 @@ bool LogReader::in_list(const char *type, const char *list[])
     return false;
 }
 
+void LogReader::initialise_fmt_map()
+{
+    for (const char **name = generated_names;
+         *name !=nullptr;
+         name++) {
+        bool found = false;
+        for (uint8_t n=0; n<ARRAY_SIZE(running_codes_log_structure); n++) {
+            if (streq(*name, running_codes_log_structure[n].name)) {
+                const uint8_t t = running_codes_log_structure[n].msg_type;
+                mapped_msgid[t] = t;
+                found = true;
+                break;
+            }
+        }
+        if (!found) {
+            if (streq(*name, "CHEK")) {
+                // HACK: CHEK is emitted using Log_Write, so doesn't
+                // have a fixed address to pre-populate the fmt-map
+                // with....
+                continue;
+            }
+            ::fprintf(stderr, "Failed to find apparently-generated-name (%s) in COMMON_LOG_STRUCTURES\n", *name);
+            abort();
+        }
+    }
+}
+
 /*
   map from an incoming format type to an outgoing format type
  */
 uint8_t LogReader::map_fmt_type(const char *name, uint8_t intype)
 {
+    if (intype == 128) {
+        // everybody's favourite FMT message...
+        return 128;
+    }
     if (mapped_msgid[intype] != 0) {
         // already mapped
         return mapped_msgid[intype];
     }
-    // see if it is in our structure list
-    for (uint8_t i=0; i<ARRAY_SIZE(log_structure); i++) {
-        if (strcmp(name, log_structure[i].name) == 0) {
-            mapped_msgid[intype] = log_structure[i].msg_type;
-            return mapped_msgid[intype];
+    for (uint8_t n=next_msgid; n<255; n++) {
+        ::fprintf(stderr, "next_msgid=%u\n", next_msgid);
+        bool already_mapped = false;
+        for (uint16_t i=0; i<sizeof(mapped_msgid); i++) {
+            if (mapped_msgid[i] == n) {
+                // already mapped - must be one of our generated names
+                already_mapped = true;
+                break;
+            }
         }
+        if (already_mapped) {
+            continue;
+        }
+        if (DataFlash_Class::instance()->msg_type_in_use(n)) {
+            continue;
+        }
+        mapped_msgid[intype] = n;
+        next_msgid = n+1;
+        break;
     }
-    // it is a new one, allocate an ID
-    mapped_msgid[intype] = next_msgid++;
+    if (mapped_msgid[intype] == 0) {
+        ::fprintf(stderr, "mapping failed\n");
+        abort();
+    }
+
     return mapped_msgid[intype];    
 }
 
 bool LogReader::save_message_type(const char *name)
 {
     bool save_message = !in_list(name, generated_names);
+    save_message = save_message && !in_list(name, log_write_names);
     if (save_chek_messages && strcmp(name, "CHEK") == 0) {
         save_message = true;
     }
@@ -148,21 +246,53 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
 	memcpy(name, f.name, 4);
 	debug("Defining log format for type (%d) (%s)\n", f.type, name);
 
-        if (save_message_type(name)) {
-            /* 
-               any messages which we won't be generating internally in
-               replay should get the original FMT header
-               We need to remap the type in the FMT header to avoid
-               conflicts with our current table
-            */
-            struct log_Format f_mapped = f;
-            f_mapped.type = map_fmt_type(name, f.type);
-            dataflash.WriteBlock(&f_mapped, sizeof(f_mapped));
+    struct LogStructure s = _log_structure[_log_structure_count++];
+    dataflash.set_num_types(_log_structure_count);
+
+    if (in_list(name, log_write_names)) {
+        debug("%s is a Log_Write-written message\n", name);
+    } else {
+        if (in_list(name, generated_names)) {
+            debug("Log format for type (%d) (%s) taken from running code\n",
+                  f.type, name);
+            bool found = false;
+            for (uint8_t n=0; n<ARRAY_SIZE(running_codes_log_structure); n++) {
+                if (streq(name, running_codes_log_structure[n].name)) {
+                    found = true;
+                    memcpy(&s, &running_codes_log_structure[n], sizeof(LogStructure));
+                    break;
+                }
+            }
+            if (!found) {
+                ::fprintf(stderr, "Expected to be able to emit an FMT for (%s), but no FMT message found in running code\n", name);
+                abort();
+            }
+        } else {
+            debug("Log format for type (%d) (%s) taken from log\n", f.type, name);
+            // generate a LogStructure entry for this FMT
+            s.msg_type = map_fmt_type(name, f.type);
+            s.msg_len = f.length;
+            s.name = f.name;
+            s.format = f.format;
+            s.labels = f.labels;
         }
 
-        if (msgparser[f.type] != NULL) {
-            return true;
-        }
+        // emit the FMT to DataFlash:
+        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.WriteCriticalBlock(&pkt, sizeof(pkt));
+    }
+
+    if (msgparser[f.type] != NULL) {
+        return true;
+    }
 
 	// map from format name to a parser subclass:
 	if (streq(name, "PARM")) {
@@ -281,6 +411,9 @@ bool LogReader::handle_msg(const struct log_Format &f, uint8_t *msg) {
     memcpy(name, f.name, 4);
 
     if (save_message_type(name)) {
+        // write this message through to output log, changing the ID
+        // present in the input log to that used for the same message
+        // name in the output log
         if (mapped_msgid[msg[2]] == 0) {
             printf("Unknown msgid %u\n", (unsigned)msg[2]);
             exit(1);
@@ -350,28 +483,3 @@ bool LogReader::set_parameter(const char *name, float value)
     }
     return true;
 }
-
-/*
-  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<ARRAY_SIZE(log_structure); n++) {
-            if (strcmp(generated_names[i], log_structure[n].name) == 0) {
-                const struct LogStructure *s = &log_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.WriteCriticalBlock(&pkt, sizeof(pkt));
-            }
-        }
-    }
-}
diff --git a/Tools/Replay/LogReader.h b/Tools/Replay/LogReader.h
index a6367086c2..f0a33e34dc 100644
--- a/Tools/Replay/LogReader.h
+++ b/Tools/Replay/LogReader.h
@@ -6,7 +6,15 @@
 class LogReader : public DataFlashFileReader
 {
 public:
-    LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, Compass &_compass, AP_GPS &_gps, AP_Airspeed &_airspeed, DataFlash_Class &_dataflash, const char **&nottypes);
+    LogReader(AP_AHRS &_ahrs,
+              AP_InertialSensor &_ins,
+              Compass &_compass,
+              AP_GPS &_gps,
+              AP_Airspeed &_airspeed,
+              DataFlash_Class &_dataflash,
+              struct LogStructure *log_structure,
+              uint8_t log_structure_count,
+              const char **&nottypes);
     bool wait_type(const char *type);
 
     const Vector3f &get_attitude(void) const { return attitude; }
@@ -32,7 +40,6 @@ public:
     static bool in_list(const char *type, const char *list[]);
 
 protected:
-    virtual void end_format_msgs(void) override;
 
 private:
     AP_AHRS &ahrs;
@@ -41,6 +48,8 @@ private:
     AP_GPS &gps;
     AP_Airspeed &airspeed;
     DataFlash_Class &dataflash;
+    struct LogStructure *_log_structure;
+    uint8_t _log_structure_count;
 
     uint8_t accel_mask;
     uint8_t gyro_mask;
@@ -72,6 +81,7 @@ private:
 
     void maybe_install_vehicle_specific_parsers();
 
+    void initialise_fmt_map();
     uint8_t map_fmt_type(const char *name, uint8_t intype);
 
     bool save_message_type(const char *name);
diff --git a/Tools/Replay/Replay.cpp b/Tools/Replay/Replay.cpp
index 8d24cef298..69d2fb90a5 100644
--- a/Tools/Replay/Replay.cpp
+++ b/Tools/Replay/Replay.cpp
@@ -100,22 +100,16 @@ void ReplayVehicle::load_parameters(void)
     AP_Param::set_default_by_name("LOG_FILE_BUFSIZE", 60);
 }
 
-static const struct LogStructure min_log_structure[] = {
-    { LOG_FORMAT_MSG, sizeof(log_Format),
-      "FMT", "BBnNZ",      "Type,Length,Name,Format,Columns", "-b---", "-----" },
-    { LOG_PARAMETER_MSG, sizeof(log_Parameter),
-      "PARM", "QNf",        "TimeUS,Name,Value", "s--", "F--" },
-    { LOG_MESSAGE_MSG, sizeof(log_Message),
-      "MSG",  "QZ",     "TimeUS,Message", "s-", "F-" },
-};
-
 void ReplayVehicle::setup(void) 
 {
     load_parameters();
-    
-    // we pass a minimal log structure, as we will be outputting the
-    // log structures we need manually, to prevent FMT duplicates
-    dataflash.Init(min_log_structure, ARRAY_SIZE(min_log_structure));
+
+    // we pass an empty log structure, filling the structure in with
+    // either the format present in the log (if we do not emit the
+    // message as a product of Replay), or the format understood in
+    // the current code (if we do emit the message in the normal
+    // places in the EKF, for example)
+    dataflash.Init(log_structure, 0);
 
     ahrs.set_compass(&compass);
     ahrs.set_fly_forward(true);
@@ -539,6 +533,7 @@ void Replay::setup()
     _vehicle.setup();
 
     inhibit_gyro_cal();
+    force_log_disarmed();
 
     if (log_info.update_rate == 400) {
         // assume copter for 400Hz
@@ -563,6 +558,12 @@ void Replay::inhibit_gyro_cal() {
         abort();
     }
 }
+void Replay::force_log_disarmed() {
+    if (!logreader.set_parameter("LOG_DISARMED", 1)) {
+        ::fprintf(stderr, "Failed to set LOG_DISARMED parameter\n");
+        abort();
+    }
+}
 
 /*
   setup user -p parameters
@@ -625,15 +626,10 @@ void Replay::write_ekf_logs(void)
 
 void Replay::read_sensors(const char *type)
 {
-    if (!done_parameters && !streq(type,"FMT") && !streq(type,"PARM")) {
-        done_parameters = true;
+    if (streq(type, "PARM")) {
         set_user_parameters();
     }
 
-    if (done_parameters && streq(type, "PARM")) {
-        set_user_parameters();
-    }
-    
     if (!done_home_init) {
         if (streq(type, "GPS") &&
             (_vehicle.gps.status() >= AP_GPS::GPS_OK_FIX_3D) && done_baro_init) {
@@ -667,13 +663,21 @@ void Replay::read_sensors(const char *type)
         }
     } 
 
+    static bool ekf_force_started = false;
+    if (!ekf_force_started) {
+        if (log_info.have_imt2 ||
+            log_info.have_imt) {
+            _vehicle.ahrs.force_ekf_start();
+            ::fprintf(stderr, "EKF force-started at %u\n", AP_HAL::micros());
+            ekf_force_started = true;
+        }
+    }
+
     bool run_ahrs = false;
     if (log_info.have_imt2) {
         run_ahrs = streq(type, "IMT2");
-        _vehicle.ahrs.force_ekf_start();
     } else if (log_info.have_imt) {
         run_ahrs = streq(type, "IMT");
-        _vehicle.ahrs.force_ekf_start();
     } else if (log_info.have_imu2) {
         run_ahrs = streq(type, "IMU2");
     } else {
@@ -740,7 +744,7 @@ void Replay::log_check_generate(void)
         velocity.x,
         velocity.y,
         velocity.z
-    };
+        );
 }
 
 
@@ -828,11 +832,15 @@ void Replay::loop()
     }
     last_timestamp = AP_HAL::micros64();
 
-    read_sensors(type);
-
-    if (!streq(type,"ATT")) {
-        return;
+    if (streq(type, "FMT")) {
+        if (!seen_non_fmt) {
+            return;
+        }
+    } else {
+        seen_non_fmt = true;
     }
+
+    read_sensors(type);
 }
 
 
diff --git a/Tools/Replay/Replay.h b/Tools/Replay/Replay.h
index 9577d93103..7755a73c84 100644
--- a/Tools/Replay/Replay.h
+++ b/Tools/Replay/Replay.h
@@ -68,6 +68,8 @@ public:
     AP_Vehicle::FixedWing aparm;
     AP_Airspeed airspeed;
     AP_Int32 unused; // logging is magic for Replay; this is unused
+    struct LogStructure log_structure[256] = {
+    };
     DataFlash_Class dataflash{unused};
 
 private:
@@ -118,14 +120,21 @@ private:
     SITL::SITL sitl;
 #endif
 
-    LogReader logreader{_vehicle.ahrs, _vehicle.ins, _vehicle.compass, _vehicle.gps, _vehicle.airspeed, _vehicle.dataflash, nottypes};
+    LogReader logreader{_vehicle.ahrs,
+            _vehicle.ins,
+            _vehicle.compass,
+            _vehicle.gps,
+            _vehicle.airspeed,
+            _vehicle.dataflash,
+            _vehicle.log_structure,
+            0,
+            nottypes};
 
     FILE *ekf1f;
     FILE *ekf2f;
     FILE *ekf3f;
     FILE *ekf4f;
 
-    bool done_parameters;
     bool done_baro_init;
     bool done_home_init;
     int32_t arm_time_ms = -1;
@@ -161,6 +170,7 @@ private:
 
     void set_ins_update_rate(uint16_t update_rate);
     void inhibit_gyro_cal();
+    void force_log_disarmed();
 
     void usage(void);
     void set_user_parameters(void);
@@ -178,6 +188,8 @@ private:
     void flush_and_exit();
 
     FILE *xfopen(const char *f, const char *mode);
+
+    bool seen_non_fmt;
 };
 
 /*