DataFlash: allow logging of 3 accels/gyros

libraries/DataFlash/DataFlash.h

@@ -381,6 +381,8 @@ struct PACKED log_Camera {
       "IMU",  "Iffffff",     "TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ" }, \
     { LOG_IMU2_MSG, sizeof(log_IMU), \
       "IMU2",  "Iffffff",     "TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ" }, \
+    { LOG_IMU3_MSG, sizeof(log_IMU), \
+      "IMU3",  "Iffffff",     "TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ" }, \
     { LOG_MESSAGE_MSG, sizeof(log_Message), \
       "MSG",  "Z",     "Message"}, \
     { LOG_RCIN_MSG, sizeof(log_RCIN), \
@@ -434,6 +436,7 @@ struct PACKED log_Camera {
 #define LOG_RADIO_MSG	  146
 #define LOG_ATRP_MSG      147
 #define LOG_CAMERA_MSG    148
+#define LOG_IMU3_MSG	  149
 
 // message types 200 to 210 reversed for GPS driver use
 // message types 211 to 220 reversed for autotune use

libraries/DataFlash/LogFile.cpp

@@ -798,6 +798,22 @@ void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins)
         accel_z : accel2.z
     };
     WriteBlock(&pkt2, sizeof(pkt2));
+    if (ins.get_gyro_count() < 3 && ins.get_accel_count() < 3) {
+        return;
+    }
+    const Vector3f &gyro3 = ins.get_gyro(2);
+    const Vector3f &accel3 = ins.get_accel(2);
+    struct log_IMU pkt3 = {
+        LOG_PACKET_HEADER_INIT(LOG_IMU3_MSG),
+        timestamp : tstamp,
+        gyro_x  : gyro3.x,
+        gyro_y  : gyro3.y,
+        gyro_z  : gyro3.z,
+        accel_x : accel3.x,
+        accel_y : accel3.y,
+        accel_z : accel3.z
+    };
+    WriteBlock(&pkt3, sizeof(pkt3));
 }
 
 // Write a text message to the log