From e440d22003fd47d0569308cf286d35ae04fe8cf9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 15 Apr 2017 21:19:36 +1000 Subject: [PATCH] DataFlash: log DCM and SITL quaternion too --- libraries/DataFlash/LogFile.cpp | 8 +++++++- libraries/DataFlash/LogStructure.h | 5 +++-- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index 917e00431d..7afc0fcc14 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -1035,9 +1035,11 @@ void DataFlash_Class::Log_Write_AHRS2(AP_AHRS &ahrs) { Vector3f euler; struct Location loc; + Quaternion quat; if (!ahrs.get_secondary_attitude(euler) || !ahrs.get_secondary_position(loc)) { return; } + ahrs.get_secondary_quaternion(quat); struct log_AHRS pkt = { LOG_PACKET_HEADER_INIT(LOG_AHR2_MSG), time_us : AP_HAL::micros64(), @@ -1046,7 +1048,11 @@ void DataFlash_Class::Log_Write_AHRS2(AP_AHRS &ahrs) yaw : (uint16_t)(wrap_360_cd(degrees(euler.z)*100)), alt : loc.alt*1.0e-2f, lat : loc.lat, - lng : loc.lng + lng : loc.lng, + q1 : quat.q1, + q2 : quat.q2, + q3 : quat.q3, + q4 : quat.q4, }; WriteBlock(&pkt, sizeof(pkt)); } diff --git a/libraries/DataFlash/LogStructure.h b/libraries/DataFlash/LogStructure.h index eb730003af..159041a029 100644 --- a/libraries/DataFlash/LogStructure.h +++ b/libraries/DataFlash/LogStructure.h @@ -203,6 +203,7 @@ struct PACKED log_AHRS { float alt; int32_t lat; int32_t lng; + float q1, q2, q3, q4; }; struct PACKED log_POS { @@ -886,11 +887,11 @@ Format characters in the format string for binary log messages { LOG_IMU3_MSG, sizeof(log_IMU), \ "IMU3", IMU_FMT, IMU_LABELS }, \ { LOG_AHR2_MSG, sizeof(log_AHRS), \ - "AHR2","QccCfLL","TimeUS,Roll,Pitch,Yaw,Alt,Lat,Lng" }, \ + "AHR2","QccCfLLffff","TimeUS,Roll,Pitch,Yaw,Alt,Lat,Lng,Q1,Q2,Q3,Q4" }, \ { LOG_POS_MSG, sizeof(log_POS), \ "POS","QLLfff","TimeUS,Lat,Lng,Alt,RelHomeAlt,RelOriginAlt" }, \ { LOG_SIMSTATE_MSG, sizeof(log_AHRS), \ - "SIM","QccCfLL","TimeUS,Roll,Pitch,Yaw,Alt,Lat,Lng" }, \ + "SIM","QccCfLLffff","TimeUS,Roll,Pitch,Yaw,Alt,Lat,Lng,Q1,Q2,Q3,Q4" }, \ { LOG_NKF1_MSG, sizeof(log_EKF1), \ "NKF1","QccCfffffffccc","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ" }, \ { LOG_NKF2_MSG, sizeof(log_NKF2), \