From d74ae535d11d614c8fe848c13b6dcb6ddfcd0f24 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 10 Oct 2016 15:44:11 +0900
Subject: [PATCH] Copter: log EKF lane switch

---
 ArduCopter/Copter.h      | 1 +
 ArduCopter/defines.h     | 1 +
 ArduCopter/ekf_check.cpp | 8 ++++++++
 3 files changed, 10 insertions(+)

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index e59f1ac3b6..716cb04e05 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -223,6 +223,7 @@ private:
 
     // system time in milliseconds of last recorded yaw reset from ekf
     uint32_t ekfYawReset_ms = 0;
+    uint8_t ekf_primary_core;
 
     // GCS selection
     AP_SerialManager serial_manager;
diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index 124002e820..06be55d065 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -407,6 +407,7 @@ enum DevOptions {
 #define ERROR_SUBSYSTEM_TERRAIN             21
 #define ERROR_SUBSYSTEM_NAVIGATION          22
 #define ERROR_SUBSYSTEM_FAILSAFE_TERRAIN    23
+#define ERROR_SUBSYSTEM_EKF_PRIMARY         24
 // general error codes
 #define ERROR_CODE_ERROR_RESOLVED           0
 #define ERROR_CODE_FAILED_TO_INITIALISE     1
diff --git a/ArduCopter/ekf_check.cpp b/ArduCopter/ekf_check.cpp
index 8cfa09f320..043a014458 100644
--- a/ArduCopter/ekf_check.cpp
+++ b/ArduCopter/ekf_check.cpp
@@ -181,4 +181,12 @@ void Copter::check_ekf_reset()
         Log_Write_Event(DATA_EKF_YAW_RESET);
     }
 
+#if AP_AHRS_NAVEKF_AVAILABLE
+    // check for change in primary EKF (log only, AC_WPNav handles position target adjustment)
+    if (EKF2.getPrimaryCoreIndex() != ekf_primary_core) {
+        ekf_primary_core = EKF2.getPrimaryCoreIndex();
+        Log_Write_Error(ERROR_SUBSYSTEM_EKF_PRIMARY, ekf_primary_core);
+        gcs_send_text_fmt(MAV_SEVERITY_WARNING, "EKF primary changed:%u\n", (unsigned)ekf_primary_core);
+    }
+#endif
 }