AP_NavEKF2: fail prearm on any core setup failure

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -666,7 +666,7 @@ bool NavEKF2::InitialiseFilter(void)
         // check if there is enough memory to create the EKF cores
         if (hal.util->available_memory() < sizeof(NavEKF2_core)*num_cores + 4096) {
             initFailure = InitFailures::NO_MEM;
-            gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF2: not enough memory");
+            gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF2: not enough memory available");
             _enable.set(0);
             return false;
         }
@@ -675,8 +675,8 @@ bool NavEKF2::InitialiseFilter(void)
         core = (NavEKF2_core*)hal.util->malloc_type(sizeof(NavEKF2_core)*num_cores, AP_HAL::Util::MEM_FAST);
         if (core == nullptr) {
             _enable.set(0);
-            initFailure = InitFailures::NO_ALLOC;
+            initFailure = InitFailures::NO_MEM;
-            gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF2: allocation failed");
+            gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF2: memory allocation failed");
             return false;
         }
 
@@ -690,16 +690,12 @@ bool NavEKF2::InitialiseFilter(void)
         for (uint8_t i=0; i<7; i++) {
             if (_imuMask & (1U<<i)) {
                 if(!core[num_cores].setup_core(i, num_cores)) {
-                    // if first core setup fails, free memory, zero the core pointer and abort
+                    // if any core setup fails, free memory, zero the core pointer and abort
-                    if (num_cores == 0) {
+                    hal.util->free_type(core, sizeof(NavEKF2_core)*num_cores, AP_HAL::Util::MEM_FAST);
-                        hal.util->free_type(core, sizeof(NavEKF2_core)*num_cores, AP_HAL::Util::MEM_FAST);
+                    core = nullptr;
-                        core = nullptr;
+                    initFailure = InitFailures::NO_SETUP;
-                        initFailure = InitFailures::NO_SETUP;
-                        return false;
-                    }
                     gcs().send_text(MAV_SEVERITY_WARNING, "NavEKF2: core %d setup failed", num_cores);
-                    // don't return here, just break the loop and finish initializing
+                    return false;
-                    break;
                 }
                 num_cores++;
             }

libraries/AP_NavEKF2/AP_NavEKF2.h

@@ -499,7 +499,6 @@ private:
         NO_IMUS, 
         NO_MASK, 
         NO_MEM, 
-        NO_ALLOC,
         NO_SETUP,
         NUM_INIT_FAILURES
     };
@@ -510,7 +509,6 @@ private:
         "EKF2: no IMUs available",
         "EKF2: EK2_IMU_MASK is zero",
         "EKF2: insufficient memory available",
-        "EKF2: memory allocation failed",
         "EKF2: core setup failed"
     };
     InitFailures initFailure;