DataFlash: log both IMU packets at once

libraries/DataFlash/DataFlash.h

@@ -47,7 +47,6 @@ public:
     void Log_Write_Parameter(const char *name, float value);
     void Log_Write_GPS(const GPS *gps, int32_t relative_alt);
     void Log_Write_IMU(const AP_InertialSensor &ins);
-    void Log_Write_IMU2(const AP_InertialSensor &ins);
     void Log_Write_RCIN(void);
     void Log_Write_RCOUT(void);
     void Log_Write_Message(const char *message);

libraries/DataFlash/LogFile.cpp

@@ -687,11 +687,12 @@ void DataFlash_Class::Log_Write_RCOUT(void)
 // Write an raw accel/gyro data packet
 void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins)
 {
+    uint32_t tstamp = hal.scheduler->millis();
     const Vector3f &gyro = ins.get_gyro();
     const Vector3f &accel = ins.get_accel();
     struct log_IMU pkt = {
         LOG_PACKET_HEADER_INIT(LOG_IMU_MSG),
-        timestamp : hal.scheduler->millis(),
+        timestamp : tstamp,
         gyro_x  : gyro.x,
         gyro_y  : gyro.y,
         gyro_z  : gyro.z,
@@ -700,27 +701,22 @@ void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins)
         accel_z : accel.z
     };
     WriteBlock(&pkt, sizeof(pkt));
-}
-
-// Write an raw accel/gyro data packet
-void DataFlash_Class::Log_Write_IMU2(const AP_InertialSensor &ins)
-{
     if (ins.get_gyro_count() < 2 && ins.get_accel_count() < 2) {
         return;
     }
-    const Vector3f &gyro = ins.get_gyro(1);
-    const Vector3f &accel = ins.get_accel(1);
-    struct log_IMU pkt = {
+    const Vector3f &gyro2 = ins.get_gyro();
+    const Vector3f &accel2 = ins.get_accel();
+    struct log_IMU pkt2 = {
         LOG_PACKET_HEADER_INIT(LOG_IMU2_MSG),
-        timestamp : hal.scheduler->millis(),
-        gyro_x  : gyro.x,
-        gyro_y  : gyro.y,
-        gyro_z  : gyro.z,
-        accel_x : accel.x,
-        accel_y : accel.y,
-        accel_z : accel.z
+        timestamp : tstamp,
+        gyro_x  : gyro2.x,
+        gyro_y  : gyro2.y,
+        gyro_z  : gyro2.z,
+        accel_x : accel2.x,
+        accel_y : accel2.y,
+        accel_z : accel2.z
     };
-    WriteBlock(&pkt, sizeof(pkt));
+    WriteBlock(&pkt2, sizeof(pkt2));
 }
 
 // Write a text message to the log