Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

DataFlash: log quaternions for EKF

Browse Source 
useful for tailsitters
This commit is contained in:
Andrew Tridgell 2017-04-15 20:37:00 +10:00
parent 248773a7c8
commit 732915d417
2 changed files with 69 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
libraries/DataFlash/LogFile.cpp
View File

@ -1247,6 +1247,20 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
};
WriteBlock(&pkt5, sizeof(pkt5));
// log quaternion
Quaternion quat;
ahrs.get_NavEKF2().getQuaternion(0, quat);
struct log_Quaternion pktq1 = {
LOG_PACKET_HEADER_INIT(LOG_NKQ1_MSG),
time_us : time_us,
q1 : quat.q1,
q2 : quat.q2,
q3 : quat.q3,
q4 : quat.q4
};
WriteBlock(&pktq1, sizeof(pktq1));
// log innovations for the second IMU if enabled
if (ahrs.get_NavEKF2().activeCores() >= 2) {
// Write 6th EKF packet
@ -1346,6 +1360,17 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
primary : (int8_t)primaryIndex
};
WriteBlock(&pkt9, sizeof(pkt9));
ahrs.get_NavEKF2().getQuaternion(1, quat);
struct log_Quaternion pktq2 = {
LOG_PACKET_HEADER_INIT(LOG_NKQ2_MSG),
time_us : time_us,
q1 : quat.q1,
q2 : quat.q2,
q3 : quat.q3,
q4 : quat.q4
};
WriteBlock(&pktq2, sizeof(pktq2));
}
// write range beacon fusion debug packet if the range value is non-zero
@ -1542,6 +1567,19 @@ void DataFlash_Class::Log_Write_EKF3(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
};
WriteBlock(&pkt5, sizeof(pkt5));
// log quaternion
Quaternion quat;
ahrs.get_NavEKF3().getQuaternion(0, quat);
struct log_Quaternion pktq1 = {
LOG_PACKET_HEADER_INIT(LOG_XKQ1_MSG),
time_us : time_us,
q1 : quat.q1,
q2 : quat.q2,
q3 : quat.q3,
q4 : quat.q4
};
WriteBlock(&pktq1, sizeof(pktq1));
// log innovations for the second IMU if enabled
if (ahrs.get_NavEKF3().activeCores() >= 2) {
// Write 6th EKF packet
@ -1640,6 +1678,17 @@ void DataFlash_Class::Log_Write_EKF3(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
};
WriteBlock(&pkt9, sizeof(pkt9));
// log quaternion
ahrs.get_NavEKF3().getQuaternion(1, quat);
struct log_Quaternion pktq2 = {
LOG_PACKET_HEADER_INIT(LOG_XKQ2_MSG),
time_us : time_us,
q1 : quat.q1,
q2 : quat.q2,
q3 : quat.q3,
q4 : quat.q4
};
WriteBlock(&pktq2, sizeof(pktq2));
}
// write range beacon fusion debug packet if the range value is non-zero
uint8_t ID;

20
libraries/DataFlash/LogStructure.h
View File

@ -390,6 +390,15 @@ struct PACKED log_NKF5 {
float posErr;
};
struct PACKED log_Quaternion {
LOG_PACKET_HEADER;
uint64_t time_us;
float q1;
float q2;
float q3;
float q4;
};
struct PACKED log_RngBcnDebug {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -786,6 +795,9 @@ struct PACKED log_Rally {
#define PID_LABELS "TimeUS,Des,P,I,D,FF,AFF"
#define PID_FMT "Qffffff"
#define QUAT_LABELS "TimeUS,Q1,Q2,Q3,Q4"
#define QUAT_FMT "Qffff"
/*
Format characters in the format string for binary log messages
@ -899,6 +911,8 @@ Format characters in the format string for binary log messages
"NKF9","QcccccfbbHBHHb","TimeUS,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI" }, \
{ LOG_NKF10_MSG, sizeof(log_RngBcnDebug), \
"NKF0","QBccCCcccccccc","TimeUS,ID,rng,innov,SIV,TR,BPN,BPE,BPD,OFH,OFL,OFN,OFE,OFD" }, \
{ LOG_NKQ1_MSG, sizeof(log_Quaternion), "NKQ1", QUAT_FMT, QUAT_LABELS }, \
{ LOG_NKQ2_MSG, sizeof(log_Quaternion), "NKQ2", QUAT_FMT, QUAT_LABELS }, \
{ LOG_XKF1_MSG, sizeof(log_EKF1), \
"XKF1","QccCfffffffccc","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ" }, \
{ LOG_XKF2_MSG, sizeof(log_NKF2a), \
@ -919,6 +933,8 @@ Format characters in the format string for binary log messages
"XKF9","QcccccfbbHBHHb","TimeUS,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI" }, \
{ LOG_XKF10_MSG, sizeof(log_RngBcnDebug), \
"XKF0","QBccCCcccccccc","TimeUS,ID,rng,innov,SIV,TR,BPN,BPE,BPD,OFH,OFL,OFN,OFE,OFD" }, \
{ LOG_XKQ1_MSG, sizeof(log_Quaternion), "XKQ1", QUAT_FMT, QUAT_LABELS }, \
{ LOG_XKQ2_MSG, sizeof(log_Quaternion), "XKQ2", QUAT_FMT, QUAT_LABELS }, \
{ LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \
"TERR","QBLLHffHH","TimeUS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded" }, \
{ LOG_GPS_UBX1_MSG, sizeof(log_Ubx1), \
@ -1104,6 +1120,8 @@ enum LogMessages {
LOG_NKF8_MSG,
LOG_NKF9_MSG,
LOG_NKF10_MSG,
LOG_NKQ1_MSG,
LOG_NKQ2_MSG,
LOG_XKF1_MSG,
LOG_XKF2_MSG,
LOG_XKF3_MSG,
@ -1114,6 +1132,8 @@ enum LogMessages {
LOG_XKF8_MSG,
LOG_XKF9_MSG,
LOG_XKF10_MSG,
LOG_XKQ1_MSG,
LOG_XKQ2_MSG,
LOG_DF_MAV_STATS,
LOG_MSG_SBPHEALTH,