Replay: automatically find message types, and improve rate detection

Tools/Replay/CheckLogs.py

@@ -34,7 +34,7 @@ def run_replay(logfile):
         opts.tolerance_euler,
         opts.tolerance_pos,
         opts.tolerance_vel)
-    run_cmd(cmd, checkfail=True)
+    run_cmd(cmd, checkfail=False)
 
 def get_log_list():
     '''get a list of log files to process'''
@@ -158,12 +158,18 @@ def check_logs():
 def create_checked_logs():
     '''create a set of CHEK logs'''
     import glob, os, sys
-    pattern = os.path.join(opts.logdir, "*.bin")
+    if os.path.isfile(opts.logdir):
-    full_file_list = glob.glob(pattern)
+        full_file_list = [opts.logdir]
+    else:
+        pattern = os.path.join(opts.logdir, "*.bin")
+        full_file_list = glob.glob(pattern)
     file_list = []
     for f in full_file_list:
         if not f.endswith("-checked.bin"):
             file_list.append(f)
+    if len(file_list) == 0:
+        print("No files to process")
+        sys.exit(1)
     for f in file_list:
         print("Processing %s" % f)
         log_list_current = set(glob.glob("logs/*.BIN"))

Tools/Replay/LR_MsgHandler.cpp

@@ -24,7 +24,7 @@ void LR_MsgHandler::wait_timestamp(uint32_t timestamp)
 void LR_MsgHandler::wait_timestamp_from_msg(uint8_t *msg)
 {
     uint64_t time_us;
-    uint64_t time_ms;
+    uint32_t time_ms;
 
     if (field_value(msg, "TimeUS", time_us)) {
         // 64-bit timestamp present - great!

Tools/Replay/LogReader.cpp

@@ -119,7 +119,8 @@ uint8_t LogReader::map_fmt_type(const char *name, uint8_t intype)
     return mapped_msgid[intype];    
 }
 
-bool LogReader::handle_log_format_msg(const struct log_Format &f) {
+bool LogReader::handle_log_format_msg(const struct log_Format &f) 
+{
 	char name[5];
 	memset(name, '\0', 5);
 	memcpy(name, f.name, 4);
@@ -137,6 +138,10 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f) {
             dataflash.WriteBlock(&f_mapped, sizeof(f_mapped));
         }
 
+        if (msgparser[f.type] != NULL) {
+            return true;
+        }
+
 	// map from format name to a parser subclass:
 	if (streq(name, "PARM")) {
             parameter_handler = new LR_MsgHandler_PARM(formats[f.type], dataflash,

Tools/Replay/LogReader.h

@@ -45,7 +45,7 @@ private:
 
     uint32_t ground_alt_cm;
 
-    class LR_MsgHandler *msgparser[LOGREADER_MAX_FORMATS];
+    class LR_MsgHandler *msgparser[LOGREADER_MAX_FORMATS] {};
 
     Vector3f attitude;
     Vector3f ahr2_attitude;

Tools/Replay/Replay.cpp

@@ -206,6 +206,14 @@ public:
     bool check_solution = false;
     const char *log_filename = NULL;
 
+    /*
+      information about a log from find_log_info
+     */
+    struct log_information {
+        uint16_t update_rate;
+        bool have_imu2;
+    } log_info {};
+
 private:
     const char *filename;
     ReplayVehicle &_vehicle;
@@ -226,10 +234,8 @@ private:
     bool done_parameters;
     bool done_baro_init;
     bool done_home_init;
-    uint16_t update_rate = 0;
     int32_t arm_time_ms = -1;
     bool ahrs_healthy;
-    bool have_imu2 = false;
     bool have_imt = false;
     bool have_imt2 = false;
     bool have_fram = false;
@@ -263,7 +269,9 @@ private:
     void read_sensors(const char *type);
     void log_check_generate();
     void log_check_solution();
+    bool show_error(const char *text, float max_error, float tolerance);
     void report_checks();
+    bool find_log_info(struct log_information &info);
 };
 
 Replay replay(replayvehicle);
@@ -271,7 +279,6 @@ Replay replay(replayvehicle);
 void Replay::usage(void)
 {
     ::printf("Options:\n");
-    ::printf("\t--rate RATE        set IMU rate in Hz\n");
     ::printf("\t--parm NAME=VALUE  set parameter NAME to VALUE\n");
     ::printf("\t--accel-mask MASK  set accel mask (1=accel1 only, 2=accel2 only, 3=both)\n");
     ::printf("\t--gyro-mask MASK   set gyro mask (1=gyro1 only, 2=gyro2 only, 3=both)\n");
@@ -300,7 +307,6 @@ void Replay::flush_dataflash(void) {
 void Replay::_parse_command_line(uint8_t argc, char * const argv[])
 {
     const struct GetOptLong::option options[] = {
-        {"rate",            true,   0, 'r'},
         {"parm",            true,   0, 'p'},
         {"param",           true,   0, 'p'},
         {"help",            false,  0, 'h'},
@@ -321,10 +327,6 @@ void Replay::_parse_command_line(uint8_t argc, char * const argv[])
     int opt;
     while ((opt = gopt.getoption()) != -1) {
 		switch (opt) {
-        case 'r':
-			update_rate = strtol(gopt.optarg, NULL, 0);
-            break;
-
         case 'g':
             logreader.set_gyro_mask(strtol(gopt.optarg, NULL, 0));
             break;
@@ -394,86 +396,87 @@ void Replay::_parse_command_line(uint8_t argc, char * const argv[])
     }
 }
 
-class IMU2Counter : public DataFlashFileReader {
+class IMUCounter : public DataFlashFileReader {
 public:
-    IMU2Counter() {}
+    IMUCounter() {}
     bool handle_log_format_msg(const struct log_Format &f);
     bool handle_msg(const struct log_Format &f, uint8_t *msg);
 
-    uint64_t last_imu2_timestamp;
+    uint64_t last_imu_timestamp;
 private:
     MsgHandler *handler;
 };
-bool IMU2Counter::handle_log_format_msg(const struct log_Format &f) {
+
-    if (!strncmp(f.name,"IMU2",4)) {
+bool IMUCounter::handle_log_format_msg(const struct log_Format &f) {
-        // an IMU2 message
+    if (!strncmp(f.name,"IMU",4)) {
+        // an IMU message
         handler = new MsgHandler(f);
     }
 
     return true;
 };
-bool IMU2Counter::handle_msg(const struct log_Format &f, uint8_t *msg) {
+
-    if (strncmp(f.name,"IMU2",4)) {
+bool IMUCounter::handle_msg(const struct log_Format &f, uint8_t *msg) {
-        // not an IMU2 message
+    if (strncmp(f.name,"IMU",4)) {
+        // not an IMU message
         return true;
     }
 
-    if (handler->field_value(msg, "TimeUS", last_imu2_timestamp)) {
+    if (handler->field_value(msg, "TimeUS", last_imu_timestamp)) {
-//        ::printf("Found timestamp %ld\n", last_imu2_timestamp);
+    } else if (handler->field_value(msg, "TimeMS", last_imu_timestamp)) {
-    } else if (handler->field_value(msg, "TimeMS", last_imu2_timestamp)) {
+        last_imu_timestamp *= 1000;
-//        ::printf("Found millisecond timestamp %ld\n", last_imu2_timestamp);
-        last_imu2_timestamp *= 1000;
     } else {
-        ::printf("Unable to find timestamp in IMU2 message");
+        ::printf("Unable to find timestamp in IMU message");
     }
     return true;
 }
 
-int find_update_rate(const char *filename) {
+/*
-    IMU2Counter reader;
+  find information about the log
+ */
+bool Replay::find_log_info(struct log_information &info) 
+{
+    IMUCounter reader;
     if (!reader.open_log(filename)) {
         perror(filename);
         exit(1);
     }
     int samplecount = 0;
     uint64_t prev = 0;
-    uint64_t samplesum = 0;
+    uint64_t smallest_delta = 0;
     prev = 0;
-    while (samplecount < 10) {
+    while (samplecount < 1000) {
         char type[5];
         if (!reader.update(type)) {
             break;
         }
-        if (streq(type, "IMU2")) {
+        if (streq(type, "IMU")) {
             if (prev == 0) {
-                prev = reader.last_imu2_timestamp;
+                prev = reader.last_imu_timestamp;
             } else {
+                uint64_t delta = reader.last_imu_timestamp - prev;
+                if (smallest_delta == 0 || delta < smallest_delta) {
+                    smallest_delta = delta;
+                }
                 samplecount++;
-                samplesum += reader.last_imu2_timestamp - prev;
-                prev = reader.last_imu2_timestamp;
             }
         }
+        if (streq(type, "IMU2") && !info.have_imu2) {
+            info.have_imu2 = true;
+        }
     }
-    if (samplecount < 10) {
+    if (smallest_delta == 0) {
-        ::printf("Unable to determine log rate - insufficient IMU2 messages?!");
+        ::printf("Unable to determine log rate - insufficient IMU messages?!");
-        exit(1);
+        return false;
     }
 
-    float rate = 1000000/int(samplesum/samplecount);
+    float rate = 1.0e6f/smallest_delta;
-    if (abs(rate - 50) < 5) {
+    if (rate < 100) {
-        return 50;
+        info.update_rate = 50;
+    } else {
+        info.update_rate = 400;
     }
-    if (abs(rate - 100) < 10) {
+    return true;
-        return 100;
-    }
-    if (abs(rate - 200) < 10) {
-        return 200;
-    }
-    if (abs(rate - 400) < 20) { // I have a log which is 10 off...
-        return 400;
-    }
-    ::printf("Unable to determine log rate - %f matches no rate\n", rate);
-    exit(1);
 }
 
 // catch floating point exceptions
@@ -510,11 +513,12 @@ void Replay::setup()
     // remember filename for reporting
     log_filename = filename;
 
-    if (update_rate == 0) {
+    if (!find_log_info(log_info)) {
-        update_rate = find_update_rate(filename);
+        printf("Update to get log information\n");
+        exit(1);
     }
 
-    hal.console->printf("Using an update rate of %u Hz\n", update_rate);
+    hal.console->printf("Using an update rate of %u Hz\n", log_info.update_rate);
 
     if (!logreader.open_log(filename)) {
         perror(filename);
@@ -522,12 +526,7 @@ void Replay::setup()
     }
 
     _vehicle.setup();
-    set_ins_update_rate(update_rate);
+    set_ins_update_rate(log_info.update_rate);
-
-    logreader.wait_type("GPS");
-    logreader.wait_type("IMU");
-    logreader.wait_type("GPS");
-    logreader.wait_type("IMU");
 
     feenableexcept(FE_INVALID | FE_OVERFLOW);
 
@@ -545,32 +544,10 @@ void Replay::setup()
     fprintf(ekf2f, "timestamp TimeMS AX AY AZ VWN VWE MN ME MD MX MY MZ\n");
     fprintf(ekf3f, "timestamp TimeMS IVN IVE IVD IPN IPE IPD IMX IMY IMZ IVT\n");
     fprintf(ekf4f, "timestamp TimeMS SV SP SH SMX SMY SMZ SVT OFN EFE FS DS\n");
-
-    ::printf("Waiting for GPS\n");
-    while (!done_home_init) {
-        char type[5];
-        if (!logreader.update(type)) {
-            break;
-        }
-        read_sensors(type);
-        if (streq(type, "GPS") &&
-            (_vehicle.gps.status() >= AP_GPS::GPS_OK_FIX_3D) &&
-            done_baro_init && !done_home_init) {
-            const Location &loc = _vehicle.gps.location();
-            ::printf("GPS Lock at %.7f %.7f %.2fm time=%.1f seconds\n", 
-                     loc.lat * 1.0e-7f, 
-                     loc.lng * 1.0e-7f,
-                     loc.alt * 0.01f,
-                     hal.scheduler->millis()*0.001f);
-            _vehicle.ahrs.set_home(loc);
-            _vehicle.compass.set_initial_location(loc.lat, loc.lng);
-            done_home_init = true;
-        }
-    }
 }
 
 void Replay::set_ins_update_rate(uint16_t _update_rate) {
-    switch (update_rate) {
+    switch (_update_rate) {
     case 50:
         _vehicle.ins.init(AP_InertialSensor::WARM_START, AP_InertialSensor::RATE_50HZ);
         break;
@@ -609,9 +586,6 @@ void Replay::read_sensors(const char *type)
         done_parameters = true;
         set_user_parameters();
     }
-    if (streq(type,"IMU2")) {
-        have_imu2 = true;
-    }
     if (use_imt && streq(type,"IMT")) {
         have_imt = true;
     }
@@ -619,6 +593,21 @@ void Replay::read_sensors(const char *type)
         have_imt2 = true;
     }
 
+    if (!done_home_init) {
+        if (streq(type, "GPS") &&
+            (_vehicle.gps.status() >= AP_GPS::GPS_OK_FIX_3D) && done_baro_init) {
+            const Location &loc = _vehicle.gps.location();
+            ::printf("GPS Lock at %.7f %.7f %.2fm time=%.1f seconds\n", 
+                     loc.lat * 1.0e-7f, 
+                     loc.lng * 1.0e-7f,
+                     loc.alt * 0.01f,
+                     hal.scheduler->millis()*0.001f);
+            _vehicle.ahrs.set_home(loc);
+            _vehicle.compass.set_initial_location(loc.lat, loc.lng);
+            done_home_init = true;
+        }
+    }
+
     if (streq(type,"GPS")) {
         _vehicle.gps.update();
         if (_vehicle.gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
@@ -656,7 +645,7 @@ void Replay::read_sensors(const char *type)
     // special handling of IMU messages as these trigger an ahrs.update()
     if (!have_fram && 
         !have_imt &&
-        ((streq(type,"IMU") && !have_imu2) || (streq(type, "IMU2") && have_imu2))) {
+        ((streq(type,"IMU") && !log_info.have_imu2) || (streq(type, "IMU2") && log_info.have_imu2))) {
         run_ahrs = true;
     }
 
@@ -980,12 +969,26 @@ void Replay::loop()
         }
     }
 
+    flush_dataflash();
+
     if (check_solution) {
         report_checks();
     }
     exit(0);
 }
 
+
+bool Replay::show_error(const char *text, float max_error, float tolerance)
+{
+    bool failed = max_error > tolerance;
+    printf("%s:\t%.2f %c %.2f\n", 
+           text,
+           max_error,
+           failed?'>':'<',
+           tolerance);
+    return failed;
+}
+
 /*
   report results of --check
  */
@@ -1006,26 +1009,11 @@ void Replay::report_checks(void)
                 check_result.max_vel_error);
         fclose(f);
     }
-    if (check_result.max_roll_error > tolerance_euler) {
+    failed |= show_error("Roll error", check_result.max_roll_error, tolerance_euler);
-        printf("Roll error %.2f > %.2f\n", check_result.max_roll_error, tolerance_euler);
+    failed |= show_error("Pitch error", check_result.max_pitch_error, tolerance_euler);
-        failed = true;
+    failed |= show_error("Yaw error", check_result.max_yaw_error, tolerance_euler);
-    }
+    failed |= show_error("Position error", check_result.max_pos_error, tolerance_pos);
-    if (check_result.max_pitch_error > tolerance_euler) {
+    failed |= show_error("Velocity error", check_result.max_vel_error, tolerance_vel);
-        printf("Pitch error %.2f > %.2f\n", check_result.max_pitch_error, tolerance_euler);
-        failed = true;
-    }
-    if (check_result.max_yaw_error > tolerance_euler) {
-        printf("Yaw error %.2f > %.2f\n", check_result.max_yaw_error, tolerance_euler);
-        failed = true;
-    }
-    if (check_result.max_pos_error > tolerance_pos) {
-        printf("Position error %.2f > %.2f\n", check_result.max_pos_error, tolerance_pos);
-        failed = true;
-    }
-    if (check_result.max_vel_error > tolerance_vel) {
-        printf("Velocity error %.2f > %.2f\n", check_result.max_vel_error, tolerance_vel);
-        failed = true;
-    }
     if (failed) {
         printf("Checks failed\n");
         exit(1);