Replay: correct various atrophications in Replay tool

Replay: tear down threads before exitting

NKQ is a generated name - don't copy it across to output

Stop whinging about presence of NKF6 and friends; we know these generated names are not going to be present in modern logs

memcpy rather than strncpy within log_FMT

Correct strings vs optionally-terminated structure entries in sanity checks

Call AP_Param::load_all() to start the parameter saving thread.  AP_Compass' init() method now saves parameters (compass reordering), and because we're disarmed we will block until the parameter is pushed onto the to-save queue; if there's no thread popping off that list we block indefinitely.

Remove duplicate definitions of various singleton objects.

Replay: write out GPS message to output log

Useful for diagnosis, but also because we struggle to find a time base
without this and the pymavlink tools take forever to work

Replay: set COMPASS_DEV_ID and COMPASS_PRIO1_ID so EKF gets mag data

Replay: avoid use of system clock; use stopped-clock only

Replay: constraint to emitting output for single core only

Tools/Replay/DataFlashFileReader.cpp

@@ -61,7 +61,7 @@ void AP_LoggerFileReader::get_packet_counts(uint64_t dest[])
     memcpy(dest, packet_counts, sizeof(packet_counts));
 }
 
-bool AP_LoggerFileReader::update(char type[5])
+bool AP_LoggerFileReader::update(char type[5], uint8_t &core)
 {
     uint8_t hdr[3];
     if (read_input(hdr, 3) != 3) {
@@ -107,5 +107,5 @@ bool AP_LoggerFileReader::update(char type[5])
     type[4] = 0;
 
     message_count++;
-    return handle_msg(f,msg);
+    return handle_msg(f, msg, core);
 }

Tools/Replay/DataFlashFileReader.h

@@ -12,10 +12,10 @@ public:
     ~AP_LoggerFileReader();
 
     bool open_log(const char *logfile);
-    bool update(char type[5]);
+    bool update(char type[5], uint8_t &core);
 
     virtual bool handle_log_format_msg(const struct log_Format &f) = 0;
-    virtual bool handle_msg(const struct log_Format &f, uint8_t *msg) = 0;
+    virtual bool handle_msg(const struct log_Format &f, uint8_t *msg, uint8_t &core) = 0;
 
     void format_type(uint16_t type, char dest[5]);
     void get_packet_counts(uint64_t dest[]);

Tools/Replay/LR_MsgHandler.cpp

@@ -2,6 +2,8 @@
 #include "LogReader.h"
 #include "Replay.h"
 
+#include <AP_HAL_Linux/Scheduler.h>
+
 #include <cinttypes>
 
 extern const AP_HAL::HAL& hal;
@@ -80,6 +82,31 @@ void LR_MsgHandler_NKF1::process_message(uint8_t *msg)
     wait_timestamp_from_msg(msg);
 }
 
+void LR_MsgHandler_NKF1::process_message(uint8_t *msg, uint8_t &core)
+{
+    wait_timestamp_from_msg(msg);
+    if (!field_value(msg, "C", core)) {
+        // 255 here is a special marker for "no core present in log".
+        // This may give us a hope of backwards-compatability.
+        core = 255;
+    }
+}
+
+void LR_MsgHandler_XKF1::process_message(uint8_t *msg)
+{
+    wait_timestamp_from_msg(msg);
+}
+
+void LR_MsgHandler_XKF1::process_message(uint8_t *msg, uint8_t &core)
+{
+    wait_timestamp_from_msg(msg);
+    if (!field_value(msg, "C", core)) {
+        // 255 here is a special marker for "no core present in log".
+        // This may give us a hope of backwards-compatability.
+        core = 255;
+    }
+}
+
 
 void LR_MsgHandler_ATT::process_message(uint8_t *msg)
 {
@@ -180,6 +207,10 @@ void LR_MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *ms
     if (status == AP_GPS::GPS_OK_FIX_3D && ground_alt_cm == 0) {
         ground_alt_cm = require_field_int32_t(msg, "Alt");
     }
+
+    // we don't call GPS update_instance which would ordinarily write
+    // these...
+    AP::logger().Write_GPS(gps_offset);
 }
 
 
@@ -408,7 +439,7 @@ void LR_MsgHandler_PARM::process_message(uint8_t *msg)
         // AP_Logger's IO only when stop_clock is called, we can
         // overflow AP_Logger's ringbuffer.  This should force us to
         // do IO:
-        hal.scheduler->stop_clock(last_timestamp_usec);
+        hal.scheduler->stop_clock(((Linux::Scheduler*)hal.scheduler)->stopped_clock_usec());
     }
 
     require_field(msg, "Name", parameter_name, parameter_name_len);

Tools/Replay/LR_MsgHandler.h

@@ -12,6 +12,12 @@ public:
                   AP_Logger &_logger,
                   uint64_t &last_timestamp_usec);
     virtual void process_message(uint8_t *msg) = 0;
+    virtual void process_message(uint8_t *msg, uint8_t &core) {
+        // base implementation just ignores the core parameter;
+        // subclasses can override to fill the core in if they feel
+        // like it.
+        process_message(msg);
+    }
 
     // state for CHEK message
     struct CheckState {
@@ -80,8 +86,19 @@ public:
 	LR_MsgHandler(_f, _logger, _last_timestamp_usec) { };
 
     void process_message(uint8_t *msg) override;
+    void process_message(uint8_t *msg, uint8_t &core) override;
 };
 
+class LR_MsgHandler_XKF1 : public LR_MsgHandler
+{
+public:
+    LR_MsgHandler_XKF1(log_Format &_f, AP_Logger &_logger,
+		    uint64_t &_last_timestamp_usec) :
+	LR_MsgHandler(_f, _logger, _last_timestamp_usec) { };
+
+    void process_message(uint8_t *msg) override;
+    void process_message(uint8_t *msg, uint8_t &core) override;
+};
 
 class LR_MsgHandler_ATT : public LR_MsgHandler
 {

Tools/Replay/LogReader.cpp

@@ -9,6 +9,8 @@
 #include <AP_InertialSensor/AP_InertialSensor.h>
 #include <AP_Logger/AP_Logger.h>
 
+#include <AP_HAL_Linux/Scheduler.h>
+
 #include "LogReader.h"
 #include <stdio.h>
 #include <unistd.h>
@@ -116,9 +118,9 @@ static const char *generated_names[] = {
     "FMT",
     "FMTU",
     "NKF1", "NKF2", "NKF3", "NKF4", "NKF5", "NKF6", "NKF7", "NKF8", "NKF9", "NKF0",
-    "NKQ1", "NKQ2",
+    "NKQ", "NKQ1", "NKQ2",
     "XKF1", "XKF2", "XKF3", "XKF4", "XKF5", "XKF6", "XKF7", "XKF8", "XKF9", "XKF0",
-    "XKQ1", "XKQ2", "XKFD", "XKV1", "XKV2",
+    "XKQ", "XKQ1", "XKQ2", "XKFD", "XKV1", "XKV2",
     "AHR2",
     "ORGN",
     "POS",
@@ -182,11 +184,13 @@ void LogReader::initialise_fmt_map()
                 // with....
                 continue;
             }
-            ::fprintf(stderr, "Failed to find apparently-generated-name (%s) in COMMON_LOG_STRUCTURES\n", *name);
             if (strncmp(*name, "NK", 2)==0 || strncmp(*name, "XK", 2) == 0) {
-                // cope with older logs
+                // cope with older logs.  Really old logs only had EKF
+                // messages, newer logs have an instance number rather
+                // than having NKF1 and NKF6.
                 continue;
             }
+            ::fprintf(stderr, "Failed to find apparently-generated-name (%s) in COMMON_LOG_STRUCTURES\n", *name);
             abort();
         }
     }
@@ -206,7 +210,7 @@ uint8_t LogReader::map_fmt_type(const char *name, uint8_t intype)
         return mapped_msgid[intype];
     }
     for (uint8_t n=next_msgid; n<255; n++) {
-        ::fprintf(stderr, "next_msgid=%u\n", next_msgid);
+        ::fprintf(stderr, "next_msgid=%u\n", n);
         bool already_mapped = false;
         for (uint16_t i=0; i<sizeof(mapped_msgid); i++) {
             if (mapped_msgid[i] == n) {
@@ -288,9 +292,9 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
         pkt.msgid = LOG_FORMAT_MSG;
         pkt.type = s.msg_type;
         pkt.length = s.msg_len;
-        strncpy(pkt.name, s.name, sizeof(pkt.name));
+        memcpy(pkt.name, s.name, sizeof(pkt.name));
-        strncpy(pkt.format, s.format, sizeof(pkt.format));
+        memcpy(pkt.format, s.format, sizeof(pkt.format));
-        strncpy(pkt.labels, s.labels, sizeof(pkt.labels));
+        memcpy(pkt.labels, s.labels, sizeof(pkt.labels));
         logger.WriteCriticalBlock(&pkt, sizeof(pkt));
     }
 
@@ -409,7 +413,7 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
         return true;
 }
 
-bool LogReader::handle_msg(const struct log_Format &f, uint8_t *msg) {
+bool LogReader::handle_msg(const struct log_Format &f, uint8_t *msg, uint8_t &core) {
     char name[5];
     memset(name, '\0', 5);
     memcpy(name, f.name, 4);
@@ -429,7 +433,7 @@ bool LogReader::handle_msg(const struct log_Format &f, uint8_t *msg) {
         // a MsgHandler would probably have found a timestamp and
         // caled stop_clock.  This runs IO, clearing logger's
         // buffer.
-        hal.scheduler->stop_clock(last_timestamp_usec);
+        hal.scheduler->stop_clock(((Linux::Scheduler*)hal.scheduler)->stopped_clock_usec());
     }
 
     LR_MsgHandler *p = msgparser[f.type];
@@ -437,27 +441,13 @@ bool LogReader::handle_msg(const struct log_Format &f, uint8_t *msg) {
 	return true;
     }
 
-    p->process_message(msg);
+    p->process_message(msg, core);
 
     maybe_install_vehicle_specific_parsers();
 
     return true;
 }
 
-bool LogReader::wait_type(const char *wtype)
-{
-    while (true) {
-        char type[5];
-        if (!update(type)) {
-            return false;
-        }
-        if (streq(type,wtype)) {
-            break;
-        }
-    }
-    return true;
-}
-
 bool LogReader::set_parameter(const char *name, float value)
 {
     enum ap_var_type var_type;

Tools/Replay/LogReader.h

@@ -15,7 +15,6 @@ public:
               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; }
     const Vector3f &get_ahr2_attitude(void) const { return ahr2_attitude; }
@@ -35,7 +34,7 @@ public:
 
     uint64_t last_timestamp_us(void) const { return last_timestamp_usec; }
     bool handle_log_format_msg(const struct log_Format &f) override;
-    bool handle_msg(const struct log_Format &f, uint8_t *msg) override;
+    bool handle_msg(const struct log_Format &f, uint8_t *msg, uint8_t &core) override;
 
     static bool in_list(const char *type, const char *list[]);
 

Tools/Replay/MsgHandler.cpp

@@ -17,6 +17,16 @@ char *xstrdup(const char *string)
     return ret;
 }
 
+char *xcalloc(uint8_t count, uint32_t len)
+{
+    char *ret = (char*)calloc(count, len);
+    if (ret == nullptr) {
+        perror("calloc");
+        fatal("calloc failed");
+    }
+    return ret;
+}
+
 void MsgHandler::add_field_type(char type, size_t size)
 {
     size_for_type_table[(type > 'A' ? (type-'A') : (type-'a'))] = size;
@@ -81,22 +91,31 @@ void MsgHandler::add_field(const char *_label, uint8_t _type, uint8_t _offset,
     next_field++;
 }
 
+char *get_string_field(char *field, uint8_t fieldlen)
+{
+    char *ret = xcalloc(1, fieldlen+1);
+    memcpy(ret, field, fieldlen);
+    return ret;
+}
+
 void MsgHandler::parse_format_fields()
 {
-    char *labels = xstrdup(f.labels);
+    char *labels = get_string_field(f.labels, sizeof(f.labels));
     char * arg = labels;
     uint8_t label_offset = 0;
     char *next_label;
     uint8_t msg_offset = 3; // 3 bytes for the header
 
+    char *format = get_string_field(f.format, ARRAY_SIZE(f.format));
+
     while ((next_label = strtok(arg, ",")) != NULL) {
-	if (label_offset > strlen(f.format)) {
+        if (label_offset > strlen(format)) {
-	    free(labels);
+            free(labels);
-	    printf("too few field times for labels %s (format=%s) (labels=%s)\n",
+            printf("too few field times for labels %s (format=%s) (labels=%s)\n",
-		   f.name, f.format, f.labels);
+                   f.name, format, labels);
-	    exit(1);
+            exit(1);
-	}
+        }
-        uint8_t field_type = f.format[label_offset];
+        uint8_t field_type = format[label_offset];
         uint8_t length = size_for_type(field_type);
         add_field(next_label, field_type, msg_offset, length);
         arg = NULL;
@@ -104,12 +123,13 @@ void MsgHandler::parse_format_fields()
         label_offset++;
     }
 
-    if (label_offset != strlen(f.format)) {
+    if (label_offset != strlen(format)) {
         printf("too few labels for format (format=%s) (labels=%s)\n",
-               f.format, f.labels);
+               format, labels);
     }
 
     free(labels);
+    free(format);
 }
 
 bool MsgHandler::field_value(uint8_t *msg, const char *label, char *ret, uint8_t retlen)

Tools/Replay/Replay.cpp

@@ -14,6 +14,7 @@
  */
 
 #include <AP_Param/AP_Param.h>
+#include <AP_HAL_Linux/Scheduler.h>
 #include <GCS_MAVLink/GCS_Dummy.h>
 #include "Parameters.h"
 #include "VehicleType.h"
@@ -38,7 +39,7 @@
 
 const AP_HAL::HAL& hal = AP_HAL::get_HAL();
 
-ReplayVehicle replayvehicle;
+static ReplayVehicle replayvehicle;
 
 struct globals globals;
 
@@ -93,13 +94,37 @@ void ReplayVehicle::load_parameters(void)
     if (!AP_Param::check_var_info()) {
         AP_HAL::panic("Bad parameter table");
     }
-    AP_Param::set_default_by_name("EK2_ENABLE", 1);
+    // Load all auto-loaded EEPROM variables - also registers thread
-    AP_Param::set_default_by_name("EK2_IMU_MASK", 1);
+    // which saves parameters, which Compass now does in its init() routine
-    AP_Param::set_default_by_name("EK3_ENABLE", 1);
+    AP_Param::load_all();
-    AP_Param::set_default_by_name("EK3_IMU_MASK", 1);
+
-    AP_Param::set_default_by_name("LOG_REPLAY", 1);
+    // this compass has always been at war with the new compass priority code
-    AP_Param::set_default_by_name("AHRS_EKF_TYPE", 2);
+    const uint32_t compass_dev_id = AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, 0, 0, AP_Compass_Backend::DevTypes::DEVTYPE_SITL);
-    AP_Param::set_default_by_name("LOG_FILE_BUFSIZE", 60);
+    // logreader.set_parameter("DEV_ID", compass_dev_id);
+    // logreader.set_parameter("PRIO1_ID", compass_dev_id);
+    AP_Param::set_default_by_name("DEV_ID", compass_dev_id);
+    AP_Param::set_default_by_name("PRIO1_ID", compass_dev_id);
+
+    const struct param_default {
+        const char *name;
+        float value;
+    } param_defaults[] {
+        { "EK2_ENABLE", 1 },
+        { "EK2_IMU_MASK", 1 },
+        { "EK3_ENABLE", 1 },
+        { "EK3_IMU_MASK", 1 },
+        { "LOG_REPLAY", 1 },
+        { "AHRS_EKF_TYPE", 2 },
+        { "LOG_FILE_BUFSIZE", 60 },
+        { "COMPASS_DEV_ID", (float)compass_dev_id },
+        { "COMPASS_PRIO1_ID", (float)compass_dev_id },
+    };
+    for (auto some_default : param_defaults) {
+        if (!AP_Param::set_default_by_name(some_default.name, some_default.value)) {
+            ::fprintf(stderr, "set default failed\n");
+            abort();
+        }
+    }
 }
 
 void ReplayVehicle::init_ardupilot(void)
@@ -131,6 +156,7 @@ void ReplayVehicle::init_ardupilot(void)
 }
 
 Replay replay(replayvehicle);
+AP_Vehicle& vehicle = replayvehicle;
 
 void Replay::usage(void)
 {
@@ -330,7 +356,7 @@ class IMUCounter : public AP_LoggerFileReader {
 public:
     IMUCounter() {}
     bool handle_log_format_msg(const struct log_Format &f) override;
-    bool handle_msg(const struct log_Format &f, uint8_t *msg) override;
+    bool handle_msg(const struct log_Format &f, uint8_t *msg, uint8_t &core) override;
 
     uint64_t last_clock_timestamp = 0;
     float last_parm_value = 0;
@@ -354,7 +380,7 @@ bool IMUCounter::handle_log_format_msg(const struct log_Format &f) {
     return true;
 };
 
-bool IMUCounter::handle_msg(const struct log_Format &f, uint8_t *msg) {
+bool IMUCounter::handle_msg(const struct log_Format &f, uint8_t *msg, uint8_t &core) {
     if (strncmp(f.name,"PARM",4) == 0) {
         // gather parameter values to check for SCHED_LOOP_RATE
         parm_handler->field_value(msg, "Name", last_parm_name, sizeof(last_parm_name));
@@ -395,7 +421,8 @@ bool Replay::find_log_info(struct log_information &info)
     const uint16_t samples_required = 1000;
     while (samplecount < samples_required) {
         char type[5];
-        if (!reader.update(type)) {
+        uint8_t core; // unused
+        if (!reader.update(type, core)) {
             break;
         }
 
@@ -513,6 +540,9 @@ void Replay::setup()
 
     set_signal_handlers();
 
+    // never use the system clock:
+    hal.scheduler->stop_clock(1);
+
     hal.console->printf("Processing log %s\n", filename);
 
     // remember filename for reporting
@@ -535,6 +565,7 @@ void Replay::setup()
     inhibit_gyro_cal();
     force_log_disarmed();
 
+    // FIXME: base this on key parameters rather than the loop rate
     if (log_info.update_rate == 400) {
         // assume copter for 400Hz
         _vehicle.ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER);
@@ -624,7 +655,7 @@ void Replay::write_ekf_logs(void)
     }
 }
 
-void Replay::read_sensors(const char *type)
+void Replay::read_sensors(const char *type, uint8_t core)
 {
     if (streq(type, "PARM")) {
         set_user_parameters();
@@ -669,7 +700,7 @@ void Replay::read_sensors(const char *type)
         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());
+            ::fprintf(stderr, "EKF force-started at %" PRIu64 "\n", AP_HAL::micros64());
             ekf_force_started = true;
         }
     }
@@ -709,8 +740,12 @@ void Replay::read_sensors(const char *type)
             log_check_solution();
         }
     }
-    
+
-    if (logmatch && (streq(type, "NKF1") || streq(type, "XKF1"))) {
+    // 255 here is a special marker for "no core present in log".
+    // This may give us a hope of backwards-compatability.
+    if (logmatch &&
+        (streq(type, "NKF1") || streq(type, "XKF1")) &&
+        (core == 0 || core == 255)) {
         write_ekf_logs();
     }
 }
@@ -788,6 +823,10 @@ void Replay::flush_and_exit()
         show_packet_counts();
     }
 
+    // If we don't tear down the threads then they continue to access
+    // global state during object destruction.
+    ((Linux::Scheduler*)hal.scheduler)->teardown();
+
     exit(0);
 }
 
@@ -811,6 +850,7 @@ void Replay::show_packet_counts()
 void Replay::loop()
 {
     char type[5];
+    uint8_t core;
 
     if (arm_time_ms >= 0 && AP_HAL::millis() > (uint32_t)arm_time_ms) {
         if (!hal.util->get_soft_armed()) {
@@ -819,19 +859,26 @@ void Replay::loop()
         }
     }
 
-    if (!logreader.update(type)) {
+    if (!logreader.update(type, core)) {
         ::printf("End of log at %.1f seconds\n", AP_HAL::millis()*0.001f);
         flush_and_exit();
     }
 
+    const uint64_t now64 = AP_HAL::micros64();
     if (last_timestamp != 0) {
-        uint64_t gap = AP_HAL::micros64() - last_timestamp;
+        if (now64 < last_timestamp) {
-        if (gap > 40000) {
+            ::printf("time going backwards?! now=%" PRIu64 " last_timestamp=%" PRIu64 "us\n",
-            ::printf("Gap in log at timestamp=%" PRIu64 " of length %" PRIu64 "us\n",
+                     now64, last_timestamp);
-                     last_timestamp, gap);
+            exit(1);
+        } else {
+            const uint64_t gap = now64 - last_timestamp;
+            if (gap > 40000) {
+                ::printf("Gap in log at timestamp=%" PRIu64 " of length %" PRIu64 "us\n",
+                         last_timestamp, gap);
+            }
         }
     }
-    last_timestamp = AP_HAL::micros64();
+    last_timestamp = now64;
 
     if (streq(type, "FMT")) {
         if (!seen_non_fmt) {
@@ -841,7 +888,7 @@ void Replay::loop()
         seen_non_fmt = true;
     }
 
-    read_sensors(type);
+    read_sensors(type, core);
 }
 
 

Tools/Replay/Replay.h

@@ -58,18 +58,15 @@ public:
     void load_parameters(void) override;
     void get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
                              uint8_t &task_count,
-                             uint32_t &log_bit) override { };
+                             uint32_t &log_bit) override {
+        tasks = nullptr;
+        task_count = 0;
+        log_bit = 0;
+    };
 
     virtual bool set_mode(const uint8_t new_mode, const ModeReason reason) override { return true; }
     virtual uint8_t get_mode() const override { return 0; }
 
-    AP_InertialSensor ins;
-    AP_Baro barometer;
-    AP_GPS gps;
-    Compass compass;
-    AP_SerialManager serial_manager;
-    RangeFinder rng;
-    AP_AHRS_NavEKF ahrs;
     AP_Vehicle::FixedWing aparm;
     AP_Airspeed airspeed;
     AP_Int32 unused; // logging is magic for Replay; this is unused
@@ -183,7 +180,7 @@ private:
 
     void usage(void);
     void set_user_parameters(void);
-    void read_sensors(const char *type);
+    void read_sensors(const char *type, uint8_t core);
     void write_ekf_logs(void);
     void log_check_generate();
     void log_check_solution();