From cc62998d40aef0ed13b267765d2a707b1cb96c6f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 3 May 2019 19:48:18 +1000 Subject: [PATCH] AP_NavEKF2: fixed bug in EKF lane selection this fixes an issue in selecting the best lane to change to when we have 3 or more EKF cores. The bug is that if the current lane is unhealthy it would always choose the last healthy lane instead of choosing the lane with the lowest score --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index b9e96a16b7..14124e3457 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -741,7 +741,7 @@ void NavEKF2::UpdateFilter(void) // If the primary core is still healthy,then switching is optional and will only be done if // a core with a significantly lower error score can be found float altErrorScore = core[coreIndex].errorScore(); - if (altCoreAvailable && (!core[primary].healthy() || altErrorScore < lowestErrorScore)) { + if (altCoreAvailable && (!core[newPrimaryIndex].healthy() || altErrorScore < lowestErrorScore)) { newPrimaryIndex = coreIndex; lowestErrorScore = altErrorScore; }