Replay: support IMT messages

Tools/Replay/LR_MsgHandler.cpp

@@ -203,6 +203,44 @@ void LR_MsgHandler_IMU::process_message(uint8_t *msg)
     update_from_msg_imu(0, msg);
 }
 
+void LR_MsgHandler_IMT_Base::update_from_msg_imt(uint8_t imu_offset, uint8_t *msg)
+{
+    wait_timestamp_from_msg(msg);
+
+    uint8_t this_imu_mask = 1 << imu_offset;
+
+    float delta_time;
+    require_field(msg, "DelT", delta_time);
+    ins.set_delta_time(delta_time);
+
+    if (gyro_mask & this_imu_mask) {
+        Vector3f d_angle;
+        require_field(msg, "DelA", d_angle);
+        ins.set_delta_angle(imu_offset, d_angle);
+    }
+    if (accel_mask & this_imu_mask) {
+        float dvt;
+        require_field(msg, "DelvT", dvt);
+        Vector3f d_velocity;
+        require_field(msg, "DelV", d_velocity);
+        ins.set_delta_velocity(imu_offset, dvt, d_velocity);
+    }
+}
+
+void LR_MsgHandler_IMT::process_message(uint8_t *msg)
+{
+  update_from_msg_imt(0, msg);
+}
+
+void LR_MsgHandler_IMT2::process_message(uint8_t *msg)
+{
+  update_from_msg_imt(1, msg);
+}
+
+void LR_MsgHandler_IMT3::process_message(uint8_t *msg)
+{
+  update_from_msg_imt(2, msg);
+}
 
 void LR_MsgHandler_MAG2::process_message(uint8_t *msg)
 {

Tools/Replay/LR_MsgHandler.h

@@ -187,7 +187,7 @@ public:
         accel_mask(_accel_mask),
         gyro_mask(_gyro_mask),
         ins(_ins) { };
-    void update_from_msg_imu(uint8_t gps_offset, uint8_t *msg);
+    void update_from_msg_imu(uint8_t imu_offset, uint8_t *msg);
 
 private:
     uint8_t &accel_mask;
@@ -235,6 +235,64 @@ public:
 };
 
 
+class LR_MsgHandler_IMT_Base : public LR_MsgHandler
+{
+public:
+    LR_MsgHandler_IMT_Base(log_Format &_f, DataFlash_Class &_dataflash,
+                        uint64_t &_last_timestamp_usec,
+                        uint8_t &_accel_mask, uint8_t &_gyro_mask,
+                        AP_InertialSensor &_ins) :
+        LR_MsgHandler(_f, _dataflash, _last_timestamp_usec),
+        accel_mask(_accel_mask),
+        gyro_mask(_gyro_mask),
+        ins(_ins) { };
+    void update_from_msg_imt(uint8_t imu_offset, uint8_t *msg);
+
+private:
+    uint8_t &accel_mask;
+    uint8_t &gyro_mask;
+    AP_InertialSensor &ins;
+};
+
+class LR_MsgHandler_IMT : public LR_MsgHandler_IMT_Base
+{
+public:
+    LR_MsgHandler_IMT(log_Format &_f, DataFlash_Class &_dataflash,
+                   uint64_t &_last_timestamp_usec,
+                   uint8_t &_accel_mask, uint8_t &_gyro_mask,
+                   AP_InertialSensor &_ins)
+        : LR_MsgHandler_IMT_Base(_f, _dataflash, _last_timestamp_usec,
+                                 _accel_mask, _gyro_mask, _ins) { };
+
+    void process_message(uint8_t *msg);
+};
+
+class LR_MsgHandler_IMT2 : public LR_MsgHandler_IMT_Base
+{
+public:
+    LR_MsgHandler_IMT2(log_Format &_f, DataFlash_Class &_dataflash,
+                   uint64_t &_last_timestamp_usec,
+                   uint8_t &_accel_mask, uint8_t &_gyro_mask,
+                   AP_InertialSensor &_ins)
+        : LR_MsgHandler_IMT_Base(_f, _dataflash, _last_timestamp_usec,
+                                  _accel_mask, _gyro_mask, _ins) { };
+
+    void process_message(uint8_t *msg);
+};
+
+class LR_MsgHandler_IMT3 : public LR_MsgHandler_IMT_Base
+{
+public:
+    LR_MsgHandler_IMT3(log_Format &_f, DataFlash_Class &_dataflash,
+                   uint64_t &_last_timestamp_usec,
+                   uint8_t &_accel_mask, uint8_t &_gyro_mask,
+                   AP_InertialSensor &_ins)
+        : LR_MsgHandler_IMT_Base(_f, _dataflash, _last_timestamp_usec,
+                                  _accel_mask, _gyro_mask, _ins) { };
+
+    void process_message(uint8_t *msg);
+};
+
 
 class LR_MsgHandler_MAG_Base : public LR_MsgHandler
 {

Tools/Replay/LogReader.cpp

@@ -132,6 +132,18 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f) {
 	    msgparser[f.type] = new LR_MsgHandler_IMU3(formats[f.type], dataflash,
                                                     last_timestamp_usec,
 						    accel_mask, gyro_mask, ins);
+	} else if (streq(name, "IMT")) {
+	    msgparser[f.type] = new LR_MsgHandler_IMT(formats[f.type], dataflash,
+                                                      last_timestamp_usec,
+                                                      accel_mask, gyro_mask, ins);
+	} else if (streq(name, "IMT2")) {
+	    msgparser[f.type] = new LR_MsgHandler_IMT2(formats[f.type], dataflash,
+                                                       last_timestamp_usec,
+                                                       accel_mask, gyro_mask, ins);
+	} else if (streq(name, "IMT3")) {
+	    msgparser[f.type] = new LR_MsgHandler_IMT3(formats[f.type], dataflash,
+                                                       last_timestamp_usec,
+                                                       accel_mask, gyro_mask, ins);
 	} else if (streq(name, "SIM")) {
 	  msgparser[f.type] = new LR_MsgHandler_SIM(formats[f.type], dataflash,
                                                  last_timestamp_usec,

Tools/Replay/Replay.cpp

@@ -114,8 +114,10 @@ private:
     uint16_t update_rate = 0;
     int32_t arm_time_ms = -1;
     bool ahrs_healthy;
-    bool have_imu2;
-    bool have_fram;
+    bool have_imu2 = false;
+    bool have_imt = false;
+    bool have_imt2 = false;
+    bool have_fram = false;
 
     void _parse_command_line(uint8_t argc, char * const argv[]);
 
@@ -473,6 +475,12 @@ void Replay::read_sensors(const char *type)
     if (streq(type,"IMU2")) {
         have_imu2 = true;
     }
+    if (streq(type,"IMT")) {
+        have_imt = true;
+    }
+    if (streq(type,"IMT2")) {
+        have_imt2 = true;
+    }
 
     if (streq(type,"GPS")) {
         gps.update();
@@ -501,8 +509,16 @@ void Replay::read_sensors(const char *type)
         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") && !have_imu2) || (streq(type, "IMU2") && have_imu2))) {
         run_ahrs = true;
     }