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

AP_NavEKF3: do logging for 3rd EKF core

Browse Source
This commit is contained in:
Siddharth Purohit 2019-07-20 14:38:49 +08:00 committed by Peter Barker
parent 1527a5b97c
commit db9f7c3dd5
1 changed files with 18 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
View File

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