From a80c0a231d128d673f1311faba311cb8a1f45ead Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 6 Mar 2017 13:56:37 +0900
Subject: [PATCH] AP_NavEKF3: attempt to set-up every core

previously we would only call setup_core for the 2nd core if the first had been setup successfully
---
 libraries/AP_NavEKF3/AP_NavEKF3.cpp | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp
index 6df9101640..725f1d36b2 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp
@@ -659,14 +659,19 @@ bool NavEKF3::InitialiseFilter(void)
     // Set up any cores that have been created
     // This specifies the IMU to be used and creates the data buffers
     // If we are unble to set up a core, return false and try again next time the function is called
+    bool core_setup_success = true;
     for (uint8_t core_index=0; core_index<num_cores; core_index++) {
         if (coreSetupRequired[core_index]) {
             coreSetupRequired[core_index] = !core[core_index].setup_core(this, coreImuIndex[core_index], core_index);
-            if(coreSetupRequired[core_index]) {
-                return false;
+            if (coreSetupRequired[core_index]) {
+                core_setup_success = false;
             }
         }
     }
+    // exit with failure if any cores could not be setup
+    if (!core_setup_success) {
+        return false;
+    }
 
     // Set the primary initially to be the lowest index
     primary = 0;