From 1858da8307a4c6a22b8fc75ed2d40fbdf1c3324c Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p-riseborough@live.com.au>
Date: Fri, 6 Nov 2015 13:11:49 +1100
Subject: [PATCH] AP_NavEKF2: Better switching in response to faults

Make selection sticky
If fault detected or unhealthy, then switch to healthy core with lowest fault score.
If no healthy core found, do not switch.
---
 libraries/AP_NavEKF2/AP_NavEKF2.cpp | 20 ++++++++++++++------
 1 file changed, 14 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
index 0da6cc7408..6c007a5d00 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
@@ -477,6 +477,9 @@ bool NavEKF2::InitialiseFilter(void)
                 num_cores++;
             }
         }
+
+        // Set the primary initially to be the lowest index
+        primary = 0;
     }
 
     // initialse the cores. We return success only if all cores
@@ -498,12 +501,17 @@ void NavEKF2::UpdateFilter(void)
         core[i].UpdateFilter();
     }
 
-    // set primary to first healthy filter
-    primary = 0;
-    for (uint8_t i=0; i<num_cores; i++) {
-        if (core[i].healthy()) {
-            primary = i;
-            break;
+    // If the current core selected has a bad fault score or is unhealthy, switch to a healthy core with the lowest fault score
+    if (core[primary].faultScore() > 0.0f || !core[primary].healthy()) {
+        float score = 1e9f;
+        for (uint8_t i=0; i<num_cores; i++) {
+            if (core[i].healthy()) {
+                float tempScore = core[i].faultScore();
+                if (tempScore < score) {
+                    primary = i;
+                    score = tempScore;
+                }
+            }
         }
     }
 }