diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index fe0f6f2c25..d530454cab 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -47,6 +47,7 @@ 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); @@ -204,6 +205,8 @@ struct PACKED log_RCOUT { "GPS", "BIHBcLLeeEefI", "Status,TimeMS,Week,NSats,HDop,Lat,Lng,RelAlt,Alt,Spd,GCrs,VZ,T" }, \ { LOG_IMU_MSG, sizeof(log_IMU), \ "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_MESSAGE_MSG, sizeof(log_Message), \ "MSG", "Z", "Message"}, \ { LOG_RCIN_MSG, sizeof(log_RCIN), \ @@ -219,6 +222,7 @@ struct PACKED log_RCOUT { #define LOG_MESSAGE_MSG 132 #define LOG_RCIN_MSG 133 #define LOG_RCOUT_MSG 134 +#define LOG_IMU2_MSG 135 #include "DataFlash_Block.h" #include "DataFlash_File.h" diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index c09627b586..753b9cf9b5 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -702,6 +702,29 @@ void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins) 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; + } + Vector3f gyro; + Vector3f accel; + ins.get_gyro_instance(1, gyro); + ins.get_accel_instance(1, accel); + struct log_IMU pkt = { + 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 + }; + WriteBlock(&pkt, sizeof(pkt)); +} + // Write a text message to the log void DataFlash_Class::Log_Write_Message(const char *message) {