AP_NavEKF2: don't try to allocate last bytes of memory to EKF2

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -106,6 +106,8 @@
 
 #endif // APM_BUILD_DIRECTORY
 
+extern const AP_HAL::HAL& hal;
+
 // Define tuning parameters
 const AP_Param::GroupInfo NavEKF2::var_info[] = {
 
@@ -452,6 +454,12 @@ bool NavEKF2::InitialiseFilter(void)
                 num_cores++;
             }
         }
+
+        if (hal.util->available_memory() < sizeof(NavEKF2_core)*num_cores + 4096) {
+            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "NavEKF2: not enough memory");
+            _enable.set(0);
+            return false;
+        }
         
         core = new NavEKF2_core[num_cores];
         if (core == nullptr) {