From 5e915fbde38f5304cf5be03ccaea89eba5f6be9c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 4 Nov 2013 16:56:55 +1100 Subject: [PATCH] DataFlash: added timestamp to IMU and VelZ to GPS logging both are very useful for analysis --- libraries/DataFlash/DataFlash.h | 6 ++++-- libraries/DataFlash/LogFile.cpp | 8 +++++--- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index da00563471..a49e0a5316 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -151,6 +151,7 @@ struct PACKED log_GPS { int32_t altitude; uint32_t ground_speed; int32_t ground_course; + float vel_z; }; struct PACKED log_Message { @@ -160,6 +161,7 @@ struct PACKED log_Message { struct PACKED log_IMU { LOG_PACKET_HEADER; + uint32_t timestamp; float gyro_x, gyro_y, gyro_z; float accel_x, accel_y, accel_z; }; @@ -170,9 +172,9 @@ struct PACKED log_IMU { { LOG_PARAMETER_MSG, sizeof(log_Parameter), \ "PARM", "Nf", "Name,Value" }, \ { LOG_GPS_MSG, sizeof(log_GPS), \ - "GPS", "BIHBcLLeeEe", "Status,TimeMS,Week,NSats,HDop,Lat,Lng,RelAlt,Alt,Spd,GCrs" }, \ + "GPS", "BIHBcLLeeEef", "Status,TimeMS,Week,NSats,HDop,Lat,Lng,RelAlt,Alt,Spd,GCrs,VelZ" }, \ { LOG_IMU_MSG, sizeof(log_IMU), \ - "IMU", "ffffff", "GyrX,GyrY,GyrZ,AccX,AccY,AccZ" }, \ + "IMU", "Iffffff", "TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ" }, \ { LOG_MESSAGE_MSG, sizeof(log_Message), \ "MSG", "Z", "Message" } diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index f28d3b3409..6775570e3a 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -615,7 +615,8 @@ void DataFlash_Class::Log_Write_GPS(const GPS *gps, int32_t relative_alt) rel_altitude : relative_alt, altitude : gps->altitude_cm, ground_speed : gps->ground_speed_cm, - ground_course : gps->ground_course_cd + ground_course : gps->ground_course_cd, + vel_z : gps->velocity_down() }; WriteBlock(&pkt, sizeof(pkt)); } @@ -624,10 +625,11 @@ void DataFlash_Class::Log_Write_GPS(const GPS *gps, int32_t relative_alt) // Write an raw accel/gyro data packet void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins) { - Vector3f gyro = ins.get_gyro(); - Vector3f accel = ins.get_accel(); + 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(), gyro_x : gyro.x, gyro_y : gyro.y, gyro_z : gyro.z,