aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

DataFlash: log DCM and SITL quaternion too

Browse Source
This commit is contained in:
Andrew Tridgell 2017-04-15 21:19:36 +10:00
parent 732915d417
commit e440d22003
2 changed files with 10 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/DataFlash/LogFile.cpp
View File

@ -1035,9 +1035,11 @@ void DataFlash_Class::Log_Write_AHRS2(AP_AHRS &ahrs)
{ {
Vector3f euler; Vector3f euler;
struct Location loc; struct Location loc;
Quaternion quat;
if (!ahrs.get_secondary_attitude(euler) || !ahrs.get_secondary_position(loc)) { if (!ahrs.get_secondary_attitude(euler) || !ahrs.get_secondary_position(loc)) {
return; return;
} }
ahrs.get_secondary_quaternion(quat);
struct log_AHRS pkt = { struct log_AHRS pkt = {
LOG_PACKET_HEADER_INIT(LOG_AHR2_MSG), LOG_PACKET_HEADER_INIT(LOG_AHR2_MSG),
time_us : AP_HAL::micros64(), 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)), yaw : (uint16_t)(wrap_360_cd(degrees(euler.z)*100)),
alt : loc.alt*1.0e-2f, alt : loc.alt*1.0e-2f,
lat : loc.lat, 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)); WriteBlock(&pkt, sizeof(pkt));
} }

5
libraries/DataFlash/LogStructure.h
View File

@ -203,6 +203,7 @@ struct PACKED log_AHRS {
float alt; float alt;
int32_t lat; int32_t lat;
int32_t lng; int32_t lng;
float q1, q2, q3, q4;
}; };
struct PACKED log_POS { struct PACKED log_POS {
@ -886,11 +887,11 @@ Format characters in the format string for binary log messages
{ LOG_IMU3_MSG, sizeof(log_IMU), \ { LOG_IMU3_MSG, sizeof(log_IMU), \
"IMU3", IMU_FMT, IMU_LABELS }, \ "IMU3", IMU_FMT, IMU_LABELS }, \
{ LOG_AHR2_MSG, sizeof(log_AHRS), \ { 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), \ { LOG_POS_MSG, sizeof(log_POS), \
"POS","QLLfff","TimeUS,Lat,Lng,Alt,RelHomeAlt,RelOriginAlt" }, \ "POS","QLLfff","TimeUS,Lat,Lng,Alt,RelHomeAlt,RelOriginAlt" }, \
{ LOG_SIMSTATE_MSG, sizeof(log_AHRS), \ { 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), \ { LOG_NKF1_MSG, sizeof(log_EKF1), \
"NKF1","QccCfffffffccc","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ" }, \ "NKF1","QccCfffffffccc","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ" }, \
{ LOG_NKF2_MSG, sizeof(log_NKF2), \ { LOG_NKF2_MSG, sizeof(log_NKF2), \