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;
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));
}

5
libraries/DataFlash/LogStructure.h
View File

@ -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), \