From 1527a5b97cf998c0ecdb53e25ea03e0d78996424 Mon Sep 17 00:00:00 2001
From: Siddharth Purohit <siddharthbharatpurohit@gmail.com>
Date: Sat, 20 Jul 2019 14:37:44 +0800
Subject: [PATCH] AP_NavEKF2: do logging for 3rd EKF core

---
 libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp | 19 +++++++++++++++++--
 1 file changed, 17 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp
index f977e091ce..6bf190d376 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp
@@ -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()
         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()
         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);
+            }            
         }
     }
 }