Replay: try to cope better with millisecond timestamps in 400Hz copter logs

Tools/Replay/LogReader.cpp

@@ -29,7 +29,8 @@ LogReader::LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro &_baro, Co
     airspeed(_airspeed),
     dataflash(_dataflash),
     accel_mask(7),
-    gyro_mask(7)
+    gyro_mask(7),
+    last_timestamp_usec(0)
 {}
 
 bool LogReader::open_log(const char *logfile)
@@ -361,6 +362,7 @@ bool LogReader::update(uint8_t &type)
         }
         memcpy(&msg, data, f.length);
         wait_timestamp(msg.timestamp);
+        //printf("IMU %lu\n", (unsigned long)msg.timestamp);
         if (gyro_mask & 1) {
             ins.set_gyro(0, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
         }
@@ -550,6 +552,7 @@ bool LogReader::update(uint8_t &type)
         wait_timestamp(msg.time_ms);
         compass.setHIL(Vector3f(msg.mag_x - msg.offset_x, msg.mag_y - msg.offset_y, msg.mag_z - msg.offset_z));
         compass.set_offsets(0, Vector3f(msg.offset_x, msg.offset_y, msg.offset_z));
+        dataflash.Log_Write_Compass(compass);
         break;
     }
 
@@ -571,7 +574,10 @@ bool LogReader::update(uint8_t &type)
 
 void LogReader::wait_timestamp(uint32_t timestamp)
 {
-    hal.scheduler->stop_clock(timestamp*1000UL);
+    uint64_t timestamp_usec = timestamp*1000UL;
+    timestamp_usec = ((timestamp_usec + 1000) / 2500) * 2500;
+    last_timestamp_usec = timestamp_usec;
+    hal.scheduler->stop_clock(timestamp_usec);
 }
 
 bool LogReader::wait_type(uint8_t wtype)

Tools/Replay/LogReader.h

@@ -40,6 +40,8 @@ public:
     void set_accel_mask(uint8_t mask) { accel_mask = mask; }
     void set_gyro_mask(uint8_t mask) { gyro_mask = mask; }
 
+    uint64_t last_timestamp_us(void) const { return last_timestamp_usec; }
+
 private:
     int fd;
     AP_AHRS &ahrs;
@@ -64,6 +66,7 @@ private:
     Vector3f sim_attitude;
     Vector3f inavpos;
     float rel_altitude;
+    uint64_t last_timestamp_usec;
 
     void wait_timestamp(uint32_t timestamp);
 

Tools/Replay/Replay.pde

@@ -97,6 +97,8 @@ static bool done_home_init;
 static uint16_t update_rate = 50;
 static uint32_t arm_time_ms;
 static bool ahrs_healthy;
+static bool have_imu2;
+static uint32_t last_imu_usec;
 
 static uint8_t num_user_parameters;
 static struct {
@@ -298,29 +300,15 @@ static void read_sensors(uint8_t type)
         done_parameters = true;
         set_user_parameters();
     }
+    if (type == LOG_IMU2_MSG) {
+        have_imu2 = true;
+    }
+
     if (type == LOG_GPS_MSG) {
         gps.update();
         if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
             ahrs.estimate_wind();
         }
-    } else if (type == LOG_IMU_MSG) {
-        uint32_t update_delta_usec = 1e6 / update_rate;
-        uint8_t update_count = update_rate>0?update_rate/50:1;
-        for (uint8_t i=0; i<update_count; i++) {
-            ahrs.update();
-            if (ahrs.get_home().lat != 0) {
-                inertial_nav.update(ins.get_delta_time());
-            }
-            hal.scheduler->stop_clock(hal.scheduler->micros() + (i+1)*update_delta_usec);
-            dataflash.Log_Write_EKF(ahrs,false);
-            dataflash.Log_Write_AHRS2(ahrs);
-            ins.set_gyro(0, ins.get_gyro());
-            ins.set_accel(0, ins.get_accel());
-            if (ahrs.healthy() != ahrs_healthy) {
-                ahrs_healthy = ahrs.healthy();
-                printf("AHRS health: %u\n", (unsigned)ahrs_healthy);
-            }
-        }
     } else if ((type == LOG_PLANE_COMPASS_MSG && LogReader.vehicle == LogReader::VEHICLE_PLANE) ||
                (type == LOG_COPTER_COMPASS_MSG && LogReader.vehicle == LogReader::VEHICLE_COPTER) ||
                (type == LOG_ROVER_COMPASS_MSG && LogReader.vehicle == LogReader::VEHICLE_ROVER)) {
@@ -335,6 +323,39 @@ static void read_sensors(uint8_t type)
             barometer.update_calibration();
         }
     }
+
+    // special handling of IMU messages as these trigger an ahrs.update()
+    if ((type == LOG_IMU_MSG && !have_imu2) || (type == LOG_IMU2_MSG && have_imu2)) {
+        uint32_t imu_deltat_usec = LogReader.last_timestamp_us() - last_imu_usec;
+        uint32_t update_delta_usec = 1e6 / update_rate;
+        if (imu_deltat_usec < update_delta_usec/2) {
+            // it is a duplicate
+            return;
+        }
+        uint32_t update_count = (imu_deltat_usec + (update_delta_usec-1)) / update_delta_usec;
+        if (update_count > 8) {
+            update_count = 8;
+        }
+        if (update_count < 1) {
+            update_count = 1;
+        }
+        last_imu_usec = LogReader.last_timestamp_us();
+        for (uint8_t i=0; i<update_count; i++) {
+            ahrs.update();
+            if (ahrs.get_home().lat != 0) {
+                inertial_nav.update(ins.get_delta_time());
+            }
+            hal.scheduler->stop_clock(hal.scheduler->micros() + update_delta_usec);
+            dataflash.Log_Write_EKF(ahrs,false);
+            dataflash.Log_Write_AHRS2(ahrs);
+            ins.set_gyro(0, ins.get_gyro());
+            ins.set_accel(0, ins.get_accel());
+            if (ahrs.healthy() != ahrs_healthy) {
+                ahrs_healthy = ahrs.healthy();
+                printf("AHRS health: %u\n", (unsigned)ahrs_healthy);
+            }
+        }
+    }
 }
 
 void loop()