Get Replay running again

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 @@ class AP_LoggerFileReader
     ~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 @@ class LR_MsgHandler : public MsgHandler {
                   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 @@ class LR_MsgHandler_NKF1 : public LR_MsgHandler
 	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));
-        strncpy(pkt.format, s.format, sizeof(pkt.format));
-        strncpy(pkt.labels, s.labels, sizeof(pkt.labels));
+        memcpy(pkt.name, s.name, sizeof(pkt.name));
+        memcpy(pkt.format, s.format, sizeof(pkt.format));
+        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,35 +433,21 @@ 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];
     if (p == NULL) {
 	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 @@ class LogReader : public AP_LoggerFileReader
               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 @@ class LogReader : public AP_LoggerFileReader
 
     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,35 +91,45 @@ 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)) {
-	    free(labels);
-	    printf("too few field times for labels %s (format=%s) (labels=%s)\n",
-		   f.name, f.format, f.labels);
-	    exit(1);
-	}
-        uint8_t field_type = f.format[label_offset];
+        if (label_offset > strlen(format)) {
+            free(labels);
+            printf("too few field times for labels %s (format=%s) (labels=%s)\n",
+                   f.name, format, labels);
+            exit(1);
+        }
+        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;
         msg_offset += length;
         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)