From 65ffe9d760e1791029f78264a1b6938c37c52dc5 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Tue, 13 Dec 2016 08:58:53 +1100
Subject: [PATCH] AP_NavEKF2: Prevent unwanted core switch on startup

Require primary core to be healthy for 10 seconds before starting selection logic
---
 libraries/AP_NavEKF2/AP_NavEKF2.cpp | 14 ++++++++++++--
 libraries/AP_NavEKF2/AP_NavEKF2.h   |  2 ++
 2 files changed, 14 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
index 0554dbd505..923e493b7e 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
@@ -523,7 +523,8 @@ NavEKF2::NavEKF2(const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng) :
     gndEffectTimeout_ms(1000),      // time in msec that baro ground effect compensation will timeout after initiation
     gndEffectBaroScaler(4.0f),      // scaler applied to the barometer observation variance when operating in ground effect
     gndGradientSigma(50),           // RMS terrain gradient percentage assumed by the terrain height estimation
-    fusionTimeStep_ms(10)           // The minimum number of msec between covariance prediction and fusion operations
+    fusionTimeStep_ms(10),          // The minimum number of msec between covariance prediction and fusion operations
+    runCoreSelection(false)         // true when the default primary core has stabilised after startup and core selection can run
 {
     AP_Param::setup_object_defaults(this, var_info);
 }
@@ -658,8 +659,17 @@ void NavEKF2::UpdateFilter(void)
     }
 
     // If the current core selected has a bad error score or is unhealthy, switch to a healthy core with the lowest fault score
+    // Don't start running the check until the primary core has started returned healthy for at least 10 seconds to avoid switching
+    // due to initial alignment fluctuations and race conditions
+    if (!runCoreSelection) {
+        static uint64_t lastUnhealthyTime_us = 0;
+        if (!core[primary].healthy() || lastUnhealthyTime_us == 0) {
+            lastUnhealthyTime_us = imuSampleTime_us;
+        }
+        runCoreSelection = (imuSampleTime_us - lastUnhealthyTime_us) > 1E7;
+    }
     float primaryErrorScore = core[primary].errorScore();
-    if (primaryErrorScore > 1.0f || !core[primary].healthy()) {
+    if ((primaryErrorScore > 1.0f || !core[primary].healthy()) && runCoreSelection) {
         float lowestErrorScore = 0.67f * primaryErrorScore;
         uint8_t newPrimaryIndex = primary; // index for new primary
         for (uint8_t coreIndex=0; coreIndex<num_cores; coreIndex++) {
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h
index ffd16d5285..3c2b24e295 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2.h
+++ b/libraries/AP_NavEKF2/AP_NavEKF2.h
@@ -405,6 +405,8 @@ private:
         float core_delta;             // the amount of D position change between cores when a change happened
     } pos_down_reset_data;
 
+    bool runCoreSelection; // true when the primary core has stabilised and the core selection logic can be started
+
     // update the yaw reset data to capture changes due to a lane switch
     // new_primary - index of the ekf instance that we are about to switch to as the primary
     // old_primary - index of the ekf instance that we are currently using as the primary