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_NavEKF2: do logging for 3rd EKF core

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

19
libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp
View File

@ -247,7 +247,7 @@ void NavEKF2::Log_Write()
Log_Write_NKF5(time_us); Log_Write_NKF5(time_us);
Log_Write_Quaternion(0, LOG_NKQ1_MSG, 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) { if (activeCores() >= 2) {
Log_Write_EKF1(1, LOG_NKF6_MSG, time_us); Log_Write_EKF1(1, LOG_NKF6_MSG, time_us);
Log_Write_NKF2(1, LOG_NKF7_MSG, time_us); Log_Write_NKF2(1, LOG_NKF7_MSG, time_us);
@ -256,6 +256,15 @@ void NavEKF2::Log_Write()
Log_Write_Quaternion(1, LOG_NKQ2_MSG, time_us); 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 // write range beacon fusion debug packet if the range value is non-zero
Log_Write_Beacon(time_us); Log_Write_Beacon(time_us);
@ -266,7 +275,13 @@ void NavEKF2::Log_Write()
struct ekf_timing timing; struct ekf_timing timing;
for (uint8_t i=0; i<activeCores(); i++) { for (uint8_t i=0; i<activeCores(); i++) {
getTimingStatistics(i, timing); 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);
}
} }
} }
} }