From 4884b2d38ade903d70a56adea514ef5ed082e285 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p-riseborough@live.com.au>
Date: Fri, 6 Nov 2015 12:15:09 +1100
Subject: [PATCH] DataFlash: Log the primary EKF2 index

---
 libraries/DataFlash/DataFlash.h | 3 ++-
 libraries/DataFlash/LogFile.cpp | 5 ++++-
 2 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h
index 92b7fa338d..54d86983e4 100644
--- a/libraries/DataFlash/DataFlash.h
+++ b/libraries/DataFlash/DataFlash.h
@@ -461,6 +461,7 @@ struct PACKED log_NKF4 {
     uint8_t timeouts;
     uint16_t solution;
     uint16_t gps;
+    uint8_t primary;
 };
 
 struct PACKED log_EKF5 {
@@ -831,7 +832,7 @@ Format characters in the format string for binary log messages
     { LOG_NKF3_MSG, sizeof(log_NKF3), \
       "NKF3","Qcccccchhhcc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT" }, \
     { LOG_NKF4_MSG, sizeof(log_NKF4), \
-      "NKF4","QcccccfbbBBHH","TimeUS,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS" }, \
+      "NKF4","QcccccfbbBBHHB","TimeUS,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI" }, \
     { LOG_NKF5_MSG, sizeof(log_EKF5), \
       "NKF5","QBhhhcccCC","TimeUS,normInnov,FIX,FIY,AFI,HAGL,offset,RI,meaRng,errHAGL" }, \
     { LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \
diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp
index cc148c8ca8..e0a7eda042 100644
--- a/libraries/DataFlash/LogFile.cpp
+++ b/libraries/DataFlash/LogFile.cpp
@@ -1367,6 +1367,8 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
     ahrs.get_NavEKF2().getFilterGpsStatus(gpsStatus);
     float tiltError;
     ahrs.get_NavEKF2().getTiltError(tiltError);
+    uint8_t primaryIndex;
+    ahrs.get_NavEKF2().getPrimaryCoreIndex(primaryIndex);
     struct log_NKF4 pkt4 = {
         LOG_PACKET_HEADER_INIT(LOG_NKF4_MSG),
         time_us : hal.scheduler->micros64(),
@@ -1381,7 +1383,8 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
         faults : (uint8_t)(faultStatus),
         timeouts : (uint8_t)(timeoutStatus),
         solution : (uint16_t)(solutionStatus.value),
-        gps : (uint16_t)(gpsStatus.value)
+        gps : (uint16_t)(gpsStatus.value),
+        primary : (uint8_t)primaryIndex
     };
     WriteBlock(&pkt4, sizeof(pkt4));