Dataflash: add IMT messages

libraries/DataFlash/DataFlash.h

@@ -67,6 +67,7 @@ public:
     void Log_Write_Parameter(const char *name, float value);
     void Log_Write_GPS(const AP_GPS &gps, uint8_t instance, int32_t relative_alt);
     void Log_Write_IMU(const AP_InertialSensor &ins);
+    void Log_Write_IMUDT(const AP_InertialSensor &ins);
     void Log_Write_Vibration(const AP_InertialSensor &ins);
     void Log_Write_RCIN(void);
     void Log_Write_RCOUT(void);
@@ -227,6 +228,14 @@ struct PACKED log_IMU {
     uint8_t gyro_health, accel_health;
 };
 
+struct PACKED log_IMUDT {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    float delta_time, delta_vel_dt;
+    float delta_ang_x, delta_ang_y, delta_ang_z;
+    float delta_vel_x, delta_vel_y, delta_vel_z;
+};
+
 struct PACKED log_Vibe {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -717,8 +726,13 @@ Format characters in the format string for binary log messages
     { LOG_BAR2_MSG, sizeof(log_BARO), \
       "BAR2",  "Qffcf", "TimeUS,Alt,Press,Temp,CRt" }, \
     { LOG_VIBE_MSG, sizeof(log_Vibe), \
-      "VIBE", "QfffIII",     "TimeUS,VibeX,VibeY,VibeZ,Clip0,Clip1,Clip2" }
-
+      "VIBE", "QfffIII",     "TimeUS,VibeX,VibeY,VibeZ,Clip0,Clip1,Clip2" }, \
+    { LOG_IMUDT_MSG, sizeof(log_IMUDT), \
+      "IMT","Qffffffff","TimeUS,DelT,DelvT,DelAX,DelAY,DelAZ,DelVX,DelVY,DelVZ" }, \
+    { LOG_IMUDT2_MSG, sizeof(log_IMUDT), \
+      "IMT2","Qffffffff","TimeUS,DelT,DelvT,DelAX,DelAY,DelAZ,DelVX,DelVY,DelVZ" }, \
+    { LOG_IMUDT3_MSG, sizeof(log_IMUDT), \
+      "IMT3","Qffffffff","TimeUS,DelT,DelvT,DelAX,DelAY,DelAZ,DelVX,DelVY,DelVZ" }
 
 #if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
 #define LOG_COMMON_STRUCTURES LOG_BASE_STRUCTURES, LOG_EXTRA_STRUCTURES
@@ -785,6 +799,9 @@ Format characters in the format string for binary log messages
 #define LOG_PIDY_MSG      181
 #define LOG_PIDA_MSG      182
 #define LOG_VIBE_MSG      183
+#define LOG_IMUDT_MSG     184
+#define LOG_IMUDT2_MSG    185
+#define LOG_IMUDT3_MSG    186
 
 // message types 200 to 210 reversed for GPS driver use
 // message types 211 to 220 reversed for autotune use

libraries/DataFlash/LogFile.cpp

@@ -868,6 +868,86 @@ void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins)
 #endif
 }
 
+// Write an accel/gyro delta time data packet
+void DataFlash_Class::Log_Write_IMUDT(const AP_InertialSensor &ins)
+{
+    float delta_t = ins.get_delta_time();
+    float delta_vel_t = ins.get_delta_velocity_dt(0);
+    Vector3f delta_angle, delta_velocity;
+    ins.get_delta_angle(0, delta_angle);
+    ins.get_delta_velocity(0, delta_velocity);
+
+    uint64_t time_us = hal.scheduler->micros64();
+    const Vector3f &gyro = ins.get_gyro(0);
+    const Vector3f &accel = ins.get_accel(0);
+    struct log_IMUDT pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_IMUDT_MSG),
+        time_us : time_us,
+        delta_time   : delta_t,
+        delta_vel_dt : delta_vel_t,
+        delta_ang_x  : delta_angle.x,
+        delta_ang_y  : delta_angle.y,
+        delta_ang_z  : delta_angle.z,
+        delta_vel_x  : delta_velocity.x,
+        delta_vel_y  : delta_velocity.y,
+        delta_vel_z  : delta_velocity.z
+    };
+    WriteBlock(&pkt, sizeof(pkt));
+    if (ins.get_gyro_count() < 2 && ins.get_accel_count() < 2) {
+        return;
+    }
+#if INS_MAX_INSTANCES > 1
+    delta_vel_t = ins.get_delta_velocity_dt(1);
+    const Vector3f &gyro2 = ins.get_gyro(1);
+    const Vector3f &accel2 = ins.get_accel(1);
+    if (!ins.get_delta_angle(1, delta_angle)) {
+        delta_angle.zero();
+    }
+    if (!ins.get_delta_velocity(1, delta_velocity)) {
+        delta_velocity.zero();
+    }
+    struct log_IMUDT pkt2 = {
+        LOG_PACKET_HEADER_INIT(LOG_IMUDT2_MSG),
+        time_us     : time_us,
+        delta_time   : delta_t,
+        delta_vel_dt : delta_vel_t,
+        delta_ang_x  : delta_angle.x,
+        delta_ang_y  : delta_angle.y,
+        delta_ang_z  : delta_angle.z,
+        delta_vel_x  : delta_velocity.x,
+        delta_vel_y  : delta_velocity.y,
+        delta_vel_z  : delta_velocity.z
+    };
+    WriteBlock(&pkt2, sizeof(pkt2));
+
+    if (ins.get_gyro_count() < 3 && ins.get_accel_count() < 3) {
+        return;
+    }
+    delta_vel_t = ins.get_delta_velocity_dt(1);
+    const Vector3f &gyro3 = ins.get_gyro(2);
+    const Vector3f &accel3 = ins.get_accel(2);
+    if (!ins.get_delta_angle(2, delta_angle)) {
+        delta_angle.zero();
+    }
+    if (!ins.get_delta_velocity(2, delta_velocity)) {
+        delta_velocity.zero();
+    }
+    struct log_IMUDT pkt3 = {
+        LOG_PACKET_HEADER_INIT(LOG_IMUDT3_MSG),
+        time_us     : time_us,
+        delta_time   : delta_t,
+        delta_vel_dt : delta_vel_t,
+        delta_ang_x  : delta_angle.x,
+        delta_ang_y  : delta_angle.y,
+        delta_ang_z  : delta_angle.z,
+        delta_vel_x  : delta_velocity.x,
+        delta_vel_y  : delta_velocity.y,
+        delta_vel_z  : delta_velocity.z
+    };
+    WriteBlock(&pkt3, sizeof(pkt3));
+#endif
+}
+
 void DataFlash_Class::Log_Write_Vibration(const AP_InertialSensor &ins)
 {
 #if INS_VIBRATION_CHECK