Replay: closer to goal of bit-accurate replay

Tools/Replay/LR_MsgHandler.cpp

@@ -125,15 +125,11 @@ void LR_MsgHandler_Event::process_message(uint8_t *msg)
 
 void LR_MsgHandler_GPS2::process_message(uint8_t *msg)
 {
-    // only LOG_GPS_MSG gives us relative altitude.  We still log
-    // the relative altitude when we get a LOG_GPS2_MESSAGE - but
-    // the value we use (probably) comes from the most recent
-    // LOG_GPS_MESSAGE message!
-    update_from_msg_gps(1, msg, false);
+    update_from_msg_gps(1, msg);
 }
 
 
-void LR_MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *msg, bool responsible_for_relalt)
+void LR_MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *msg)
 {
     uint64_t time_us;
     if (! field_value(msg, "TimeUS", time_us)) {
@@ -159,9 +155,17 @@ void LR_MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *ms
         ! field_value(msg, "numSV", nsats)) {
         field_not_found(msg, "NSats");
     }
+    uint16_t GWk;
+    uint32_t GMS;
+    if (! field_value(msg, "GWk", GWk)) {
+        field_not_found(msg, "GWk");
+    }
+    if (! field_value(msg, "GMS", GMS)) {
+        field_not_found(msg, "GMS");
+    }
     gps.setHIL(gps_offset,
                (AP_GPS::GPS_Status)status,
-               uint32_t(time_us/1000),
+               AP_GPS::time_epoch_convert(GWk, GMS),
                loc,
                vel,
                nsats,
@@ -170,22 +174,13 @@ 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");
     }
-
-    if (responsible_for_relalt) {
-        // this could possibly check for the presence of "RelAlt" label?
-        int32_t tmp;
-        if (! field_value(msg, "RAlt", tmp)) {
-            tmp = require_field_int32_t(msg, "RelAlt");
-        }
-        rel_altitude = 0.01f * tmp;
-    }
 }
 
 
 
 void LR_MsgHandler_GPS::process_message(uint8_t *msg)
 {
-    update_from_msg_gps(0, msg, true);
+    update_from_msg_gps(0, msg);
 }
 
 

Tools/Replay/LR_MsgHandler.h

@@ -141,18 +141,16 @@ class LR_MsgHandler_GPS_Base : public LR_MsgHandler
 public:
     LR_MsgHandler_GPS_Base(log_Format &_f, DataFlash_Class &_dataflash,
                            uint64_t &_last_timestamp_usec, AP_GPS &_gps,
-                           uint32_t &_ground_alt_cm, float &_rel_altitude)
+                           uint32_t &_ground_alt_cm)
         : LR_MsgHandler(_f, _dataflash, _last_timestamp_usec),
-          gps(_gps), ground_alt_cm(_ground_alt_cm),
-          rel_altitude(_rel_altitude) { };
+          gps(_gps), ground_alt_cm(_ground_alt_cm) { };
 
 protected:
-    void update_from_msg_gps(uint8_t imu_offset, uint8_t *data, bool responsible_for_relalt);
+    void update_from_msg_gps(uint8_t imu_offset, uint8_t *data);
 
 private:
     AP_GPS &gps;
     uint32_t &ground_alt_cm;
-    float &rel_altitude;
 };
 
 
@@ -161,17 +159,16 @@ class LR_MsgHandler_GPS : public LR_MsgHandler_GPS_Base
 public:
     LR_MsgHandler_GPS(log_Format &_f, DataFlash_Class &_dataflash,
                    uint64_t &_last_timestamp_usec, AP_GPS &_gps,
-                   uint32_t &_ground_alt_cm, float &_rel_altitude)
+                   uint32_t &_ground_alt_cm)
         : LR_MsgHandler_GPS_Base(_f, _dataflash,_last_timestamp_usec,
-                              _gps, _ground_alt_cm, _rel_altitude),
-          gps(_gps), ground_alt_cm(_ground_alt_cm), rel_altitude(_rel_altitude) { };
+                              _gps, _ground_alt_cm),
+        gps(_gps), ground_alt_cm(_ground_alt_cm) { };
 
     void process_message(uint8_t *msg);
 
 private:
     AP_GPS &gps;
     uint32_t &ground_alt_cm;
-    float &rel_altitude;
 };
 
 // it would be nice to use the same parser for both GPS message types
@@ -183,16 +180,14 @@ class LR_MsgHandler_GPS2 : public LR_MsgHandler_GPS_Base
 public:
     LR_MsgHandler_GPS2(log_Format &_f, DataFlash_Class &_dataflash,
                     uint64_t &_last_timestamp_usec, AP_GPS &_gps,
-                    uint32_t &_ground_alt_cm, float &_rel_altitude)
+                    uint32_t &_ground_alt_cm)
         : LR_MsgHandler_GPS_Base(_f, _dataflash, _last_timestamp_usec,
-                                 _gps, _ground_alt_cm,
-                                 _rel_altitude), gps(_gps),
-          ground_alt_cm(_ground_alt_cm), rel_altitude(_rel_altitude) { };
+                                 _gps, _ground_alt_cm), gps(_gps),
+        ground_alt_cm(_ground_alt_cm) { };
     virtual void process_message(uint8_t *msg);
 private:
     AP_GPS &gps;
     uint32_t &ground_alt_cm;
-    float &rel_altitude;
 };
 
 

Tools/Replay/LogReader.cpp

@@ -88,7 +88,12 @@ LR_MsgHandler_PARM *parameter_handler;
 static const char *generated_names[] = { "EKF1", "EKF2", "EKF3", "EKF4", "EKF5",
                                          "NKF1", "NKF2", "NKF3", "NKF4", "NKF5",
                                          "NKF6", "NKF7", "NKF8", "NKF9", "NKF10",
-                                         "AHR2", "POS", "CHEK", NULL };
+                                         "AHR2", "POS", "CHEK",
+                                         "IMT", "IMT2",
+                                         "MAG", "MAG2",
+                                         "BARO", "BAR2",
+                                         "GPS",
+                                         "NKA", "NKV", NULL };
 
 /*
   see if a type is in a list of types
@@ -166,15 +171,13 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
 	    msgparser[f.type] = parameter_handler;
 	} else if (streq(name, "GPS")) {
 	    msgparser[f.type] = new LR_MsgHandler_GPS(formats[f.type],
-						   dataflash,
-                                                   last_timestamp_usec,
-                                                   gps, ground_alt_cm,
-                                                   rel_altitude);
+                                                      dataflash,
+                                                      last_timestamp_usec,
+                                                      gps, ground_alt_cm);
 	} else if (streq(name, "GPS2")) {
 	    msgparser[f.type] = new LR_MsgHandler_GPS2(formats[f.type], dataflash,
-                                                    last_timestamp_usec,
-						    gps, ground_alt_cm,
-						    rel_altitude);
+                                                       last_timestamp_usec,
+                                                       gps, ground_alt_cm);
 	} else if (streq(name, "MSG")) {
 	    msgparser[f.type] = new LR_MsgHandler_MSG(formats[f.type], dataflash,
                                                    last_timestamp_usec,

Tools/Replay/Replay.cpp

@@ -196,6 +196,7 @@ void ReplayVehicle::setup(void)
     ahrs.set_wind_estimation(true);
     ahrs.set_correct_centrifugal(true);
     ahrs.set_ekf_use(true);
+
     EKF2.set_enable(true);
                         
     printf("Starting disarmed\n");
@@ -228,7 +229,9 @@ public:
      */
     struct log_information {
         uint16_t update_rate;
-        bool have_imu2;
+        bool have_imu2:1;
+        bool have_imt:1;
+        bool have_imt2:1;
     } log_info {};
 
 private:
@@ -253,9 +256,6 @@ private:
     bool done_home_init;
     int32_t arm_time_ms = -1;
     bool ahrs_healthy;
-    bool have_imt = false;
-    bool have_imt2 = false;
-    bool have_fram = false;
     bool use_imt = true;
     bool check_generate = false;
     float tolerance_euler = 3;
@@ -552,9 +552,15 @@ bool Replay::find_log_info(struct log_information &info)
             }
         }
 
-        if (streq(type, "IMU2") && !info.have_imu2) {
+        if (streq(type, "IMU2")) {
             info.have_imu2 = true;
         }
+        if (streq(type, "IMT")) {
+            info.have_imt = true;
+        }
+        if (streq(type, "IMT2")) {
+            info.have_imt2 = true;
+        }
     }
     if (smallest_delta == 0) {
         ::printf("Unable to determine log rate - insufficient IMU/IMT messages? (need=%d got=%d)", samples_required, samplecount);
@@ -623,6 +629,17 @@ void Replay::setup()
     _vehicle.setup();
 
     inhibit_gyro_cal();
+
+    if (log_info.update_rate == 400) {
+        // assume copter for 400Hz
+        _vehicle.ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER);
+        _vehicle.ahrs.set_fly_forward(false);
+    } else if (log_info.update_rate == 50) {
+        // assume copter for 400Hz
+        _vehicle.ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING);
+        _vehicle.ahrs.set_fly_forward(true);
+    }
+    
     set_ins_update_rate(log_info.update_rate);
 
     feenableexcept(FE_INVALID | FE_OVERFLOW);
@@ -635,7 +652,7 @@ void Replay::setup()
     ekf3f = fopen("EKF3.dat", "w");
     ekf4f = fopen("EKF4.dat", "w");
 
-    fprintf(plotf, "time SIM.Roll SIM.Pitch SIM.Yaw BAR.Alt FLIGHT.Roll FLIGHT.Pitch FLIGHT.Yaw FLIGHT.dN FLIGHT.dE FLIGHT.Alt AHR2.Roll AHR2.Pitch AHR2.Yaw DCM.Roll DCM.Pitch DCM.Yaw EKF.Roll EKF.Pitch EKF.Yaw INAV.dN INAV.dE INAV.Alt EKF.dN EKF.dE EKF.Alt\n");
+    fprintf(plotf, "time SIM.Roll SIM.Pitch SIM.Yaw BAR.Alt FLIGHT.Roll FLIGHT.Pitch FLIGHT.Yaw FLIGHT.dN FLIGHT.dE AHR2.Roll AHR2.Pitch AHR2.Yaw DCM.Roll DCM.Pitch DCM.Yaw EKF.Roll EKF.Pitch EKF.Yaw INAV.dN INAV.dE INAV.Alt EKF.dN EKF.dE EKF.Alt\n");
     fprintf(plotf2, "time E1 E2 E3 VN VE VD PN PE PD GX GY GZ WN WE MN ME MD MX MY MZ E1ref E2ref E3ref\n");
     fprintf(ekf1f, "timestamp TimeMS Roll Pitch Yaw VN VE VD PN PE PD GX GY GZ\n");
     fprintf(ekf2f, "timestamp TimeMS AX AY AZ VWN VWE MN ME MD MX MY MZ\n");
@@ -680,12 +697,6 @@ void Replay::read_sensors(const char *type)
         done_parameters = true;
         set_user_parameters();
     }
-    if (use_imt && streq(type,"IMT")) {
-        have_imt = true;
-    }
-    if (use_imt && streq(type,"IMT2")) {
-        have_imt2 = true;
-    }
 
     if (!done_home_init) {
         if (streq(type, "GPS") &&
@@ -721,26 +732,16 @@ void Replay::read_sensors(const char *type)
     } 
 
     bool run_ahrs = false;
-    if (streq(type,"FRAM")) {
-        if (!have_fram) {
-            have_fram = true;
-            printf("Have FRAM framing\n");
-        }
-        run_ahrs = true;
-    }
-
-    if (have_imt) {
-        if ((streq(type,"IMT") && !have_imt2) ||
-            (streq(type,"IMT2") && have_imt2)) {
-            run_ahrs = true;
-        }
-    }
-
-    // special handling of IMU messages as these trigger an ahrs.update()
-    if (!have_fram && 
-        !have_imt &&
-        ((streq(type,"IMU") && !log_info.have_imu2) || (streq(type, "IMU2") && log_info.have_imu2))) {
-        run_ahrs = true;
+    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 {
+        run_ahrs = streq(type, "IMU");
     }
 
     /*
@@ -903,7 +904,7 @@ void Replay::loop()
             float temp = degrees(ekf_euler.z);
 
             if (temp < 0.0f) temp = temp + 360.0f;
-            fprintf(plotf, "%.3f %.1f %.1f %.1f %.2f %.1f %.1f %.1f %.2f %.2f %.2f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.2f %.2f %.2f %.2f %.2f %.2f\n",
+            fprintf(plotf, "%.3f %.1f %.1f %.1f %.2f %.1f %.1f %.1f %.2f %.2f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.2f %.2f %.2f %.2f %.2f %.2f\n",
                     AP_HAL::millis() * 0.001f,
                     logreader.get_sim_attitude().x,
                     logreader.get_sim_attitude().y,
@@ -914,7 +915,6 @@ void Replay::loop()
                     wrap_180_cd(logreader.get_attitude().z*100)*0.01f,
                     logreader.get_inavpos().x,
                     logreader.get_inavpos().y,
-                    logreader.get_relalt(),
                     logreader.get_ahr2_attitude().x,
                     logreader.get_ahr2_attitude().y,
                     wrap_180_cd(logreader.get_ahr2_attitude().z*100)*0.01f,