From 08658909adf0ca19528e87863ac0b9c413e442b4 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 9 Dec 2013 16:05:17 +1100
Subject: [PATCH] DataFlash: log both IMU packets at once

---
 libraries/DataFlash/DataFlash.h |  1 -
 libraries/DataFlash/LogFile.cpp | 30 +++++++++++++-----------------
 2 files changed, 13 insertions(+), 18 deletions(-)

diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h
index d530454cab..1a4f13abb5 100644
--- a/libraries/DataFlash/DataFlash.h
+++ b/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);
diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp
index 856102d9ee..7df59a947b 100644
--- a/libraries/DataFlash/LogFile.cpp
+++ b/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