Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Logging for 3rd EKF core #11857

Merged
merged 3 commits into from Jul 23, 2019
Merged

Logging for 3rd EKF core #11857

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
2a0e502
AP_Logger: add logging for 3rd instance of EKF core
bugobliterator Jul 20, 2019
70c2acc
AP_NavEKF2: do logging for 3rd EKF core
bugobliterator Jul 20, 2019
e6e4481
AP_NavEKF3: do logging for 3rd EKF core
bugobliterator Jul 20, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
28 changes: 28 additions & 0 deletions libraries/AP_Logger/LogStructure.h
View file
Open in desktop
Expand Up @@ -1386,8 +1386,17 @@ struct PACKED log_Arm_Disarm {
"NKF9","QcccccfbbHBHHb","TimeUS,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI", "s-----???-----", "F-----???-----" }, \
{ LOG_NKF10_MSG, sizeof(log_RngBcnDebug), \
"NKF0","QBccCCcccccccc","TimeUS,ID,rng,innov,SIV,TR,BPN,BPE,BPD,OFH,OFL,OFN,OFE,OFD", "s-m---mmmmmmmm", "F-B---BBBBBBBB" }, \
{ LOG_NKF11_MSG, sizeof(log_EKF1), \
"NK11","QccCfffffffccce","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ,OH", "sddhnnn-mmmkkkm", "FBBB000-000BBBB" }, \
{ LOG_NKF12_MSG, sizeof(log_NKF2), \
"NK12","QbccccchhhhhhB","TimeUS,AZbias,GSX,GSY,GSZ,VWN,VWE,MN,ME,MD,MX,MY,MZ,MI", "s----nnGGGGGG-", "F----BBCCCCCC-" }, \
{ LOG_NKF13_MSG, sizeof(log_NKF3), \
"NK13","Qcccccchhhcc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT", "snnnmmmGGGd?", "FBBBBBBCCCBB" }, \
{ LOG_NKF14_MSG, sizeof(log_NKF4), \
"NK14","QcccccfbbHBHHb","TimeUS,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI", "s-----???-----", "F-----???-----" }, \
{ LOG_NKQ1_MSG, sizeof(log_Quaternion), "NKQ1", QUAT_FMT, QUAT_LABELS, QUAT_UNITS, QUAT_MULTS }, \
{ LOG_NKQ2_MSG, sizeof(log_Quaternion), "NKQ2", QUAT_FMT, QUAT_LABELS, QUAT_UNITS, QUAT_MULTS }, \
{ LOG_NKQ3_MSG, sizeof(log_Quaternion), "NKQ3", QUAT_FMT, QUAT_LABELS, QUAT_UNITS, QUAT_MULTS }, \
{ LOG_XKF1_MSG, sizeof(log_EKF1), \
"XKF1","QccCfffffffccce","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ,OH", "sddhnnnnmmmkkkm", "FBBB0000000BBBB" }, \
{ LOG_XKF2_MSG, sizeof(log_NKF2a), \
Expand All @@ -1408,8 +1417,17 @@ struct PACKED log_Arm_Disarm {
"XKF9","QcccccfbbHBHHb","TimeUS,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI", "s-----???-----", "F-----???-----" }, \
{ LOG_XKF10_MSG, sizeof(log_RngBcnDebug), \
"XKF0","QBccCCcccccccc","TimeUS,ID,rng,innov,SIV,TR,BPN,BPE,BPD,OFH,OFL,OFN,OFE,OFD", "s-m---mmmmmmmm", "F-B---BBBBBBBB" }, \
{ LOG_XKF11_MSG, sizeof(log_EKF1), \
"XK11","QccCfffffffccce","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ,OH", "sddhnnn-mmmkkkm", "FBBB000-000BBBB" }, \
{ LOG_XKF12_MSG, sizeof(log_NKF2a), \
"XK12","QccccchhhhhhB","TimeUS,AX,AY,AZ,VWN,VWE,MN,ME,MD,MX,MY,MZ,MI", "s---nnGGGGGG-", "F---BBCCCCCC-" }, \
{ LOG_XKF13_MSG, sizeof(log_NKF3), \
"XK13","Qcccccchhhcc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT", "snnnmmmGGGd?", "FBBBBBBCCCBB" }, \
{ LOG_XKF14_MSG, sizeof(log_NKF4), \
"XK14","QcccccfbbHBHHb","TimeUS,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI", "s-----???-----", "F-----???-----" }, \
{ LOG_XKQ1_MSG, sizeof(log_Quaternion), "XKQ1", QUAT_FMT, QUAT_LABELS, QUAT_UNITS, QUAT_MULTS }, \
{ LOG_XKQ2_MSG, sizeof(log_Quaternion), "XKQ2", QUAT_FMT, QUAT_LABELS, QUAT_UNITS, QUAT_MULTS }, \
{ LOG_XKQ3_MSG, sizeof(log_Quaternion), "XKQ3", QUAT_FMT, QUAT_LABELS, QUAT_UNITS, QUAT_MULTS }, \
{ LOG_XKFD_MSG, sizeof(log_ekfBodyOdomDebug), \
"XKFD","Qffffff","TimeUS,IX,IY,IZ,IVX,IVY,IVZ", "s------", "F------" }, \
{ LOG_XKV1_MSG, sizeof(log_ekfStateVar), \
Expand Down Expand Up @@ -1554,8 +1572,13 @@ enum LogMessages : uint8_t {
LOG_NKF8_MSG,
LOG_NKF9_MSG,
LOG_NKF10_MSG,
LOG_NKF11_MSG,
LOG_NKF12_MSG,
LOG_NKF13_MSG,
LOG_NKF14_MSG,
LOG_NKQ1_MSG,
LOG_NKQ2_MSG,
LOG_NKQ3_MSG,
LOG_XKF1_MSG,
LOG_XKF2_MSG,
LOG_XKF3_MSG,
Expand All @@ -1566,8 +1589,13 @@ enum LogMessages : uint8_t {
LOG_XKF8_MSG,
LOG_XKF9_MSG,
LOG_XKF10_MSG,
LOG_XKF11_MSG,
LOG_XKF12_MSG,
LOG_XKF13_MSG,
LOG_XKF14_MSG,
LOG_XKQ1_MSG,
LOG_XKQ2_MSG,
LOG_XKQ3_MSG,
LOG_XKFD_MSG,
LOG_XKV1_MSG,
LOG_XKV2_MSG,
Expand Down
19 changes: 17 additions & 2 deletions libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp
View file
Open in desktop
Expand Up @@ -247,7 +247,7 @@ void NavEKF2::Log_Write()
Log_Write_NKF5(time_us);
Log_Write_Quaternion(0, LOG_NKQ1_MSG, time_us);

// log innovations for the second IMU if enabled
// log EKF state info for the second EFK core if enabled
if (activeCores() >= 2) {
Log_Write_EKF1(1, LOG_NKF6_MSG, time_us);
Log_Write_NKF2(1, LOG_NKF7_MSG, time_us);
Expand All @@ -256,6 +256,15 @@ void NavEKF2::Log_Write()
Log_Write_Quaternion(1, LOG_NKQ2_MSG, time_us);
}

// log EKF state info for the third EFK core if enabled
if (activeCores() >= 3) {
Log_Write_EKF1(2, LOG_NKF11_MSG, time_us);
Log_Write_NKF2(2, LOG_NKF12_MSG, time_us);
Log_Write_NKF3(2, LOG_NKF13_MSG, time_us);
Log_Write_NKF4(2, LOG_NKF14_MSG, time_us);
Log_Write_Quaternion(2, LOG_NKQ3_MSG, time_us);
}

// write range beacon fusion debug packet if the range value is non-zero
Log_Write_Beacon(time_us);

Expand All @@ -266,7 +275,13 @@ void NavEKF2::Log_Write()
struct ekf_timing timing;
for (uint8_t i=0; i<activeCores(); i++) {
getTimingStatistics(i, timing);
AP::logger().Write_EKF_Timing(i==0?"NKT1":"NKT2", time_us, timing);
if (i == 0) {
AP::logger().Write_EKF_Timing("NKT1", time_us, timing);
} else if (i == 1) {
AP::logger().Write_EKF_Timing("NKT2", time_us, timing);
} else if (i == 2) {
AP::logger().Write_EKF_Timing("NKT3", time_us, timing);
}
}
}
}
20 changes: 18 additions & 2 deletions libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
View file
Open in desktop
Expand Up @@ -311,7 +311,7 @@ void NavEKF3::Log_Write()
Log_Write_NKF5(time_us);
Log_Write_Quaternion(0, LOG_XKQ1_MSG, time_us);

// log innovations for the second IMU if enabled
// log EKF state info for the second EFK core if enabled
if (activeCores() >= 2) {
Log_Write_EKF1(1, LOG_XKF6_MSG, time_us);
Log_Write_NKF2a(1, LOG_XKF7_MSG, time_us);
Expand All @@ -320,6 +320,16 @@ void NavEKF3::Log_Write()
Log_Write_Quaternion(1, LOG_XKQ2_MSG, time_us);
}

// log EKF state info for the third EFK core if enabled
if (activeCores() >= 3) {
Log_Write_EKF1(2, LOG_XKF11_MSG, time_us);
Log_Write_NKF2a(2, LOG_XKF12_MSG, time_us);
Log_Write_NKF3(2, LOG_XKF13_MSG, time_us);
Log_Write_NKF4(2, LOG_XKF14_MSG, time_us);
Log_Write_Quaternion(2, LOG_XKQ3_MSG, time_us);
}


// write range beacon fusion debug packet if the range value is non-zero
Log_Write_Beacon(time_us);

Expand All @@ -336,7 +346,13 @@ void NavEKF3::Log_Write()
struct ekf_timing timing;
for (uint8_t i=0; i<activeCores(); i++) {
getTimingStatistics(i, timing);
AP::logger().Write_EKF_Timing(i==0?"XKT1":"XKT2", time_us, timing);
if (i == 0) {
AP::logger().Write_EKF_Timing("XKT1", time_us, timing);
} else if (i == 1) {
AP::logger().Write_EKF_Timing("XKT2", time_us, timing);
} else if (i == 2) {
AP::logger().Write_EKF_Timing("XKT3", time_us, timing);
}
}
}
}
Expand Down