Copter: log EKF lane switch

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;

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

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
 }