|
|
|
@ -247,7 +247,7 @@ void NavEKF2::Log_Write()
@@ -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); |
|
|
|
@ -256,6 +256,15 @@ void NavEKF2::Log_Write()
@@ -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); |
|
|
|
|
|
|
|
|
@ -266,7 +275,13 @@ void NavEKF2::Log_Write()
@@ -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); |
|
|
|
|
}
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|