From e6d8e329d5e7bb2a580d913aebcee00169ae7ac1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Jun 2014 14:04:55 +1000 Subject: [PATCH] DataFlash: allow logging of 3 accels/gyros --- libraries/DataFlash/DataFlash.h | 3 +++ libraries/DataFlash/LogFile.cpp | 16 ++++++++++++++++ 2 files changed, 19 insertions(+) diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index ebedfe4176..aa3d229d16 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/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 diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index bafdf9aad8..c4adc426af 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/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