diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 4aa66c461f..bb916d5882 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -621,6 +621,11 @@ void NavEKF2::check_log_write(void) // Initialise the filter bool NavEKF2::InitialiseFilter(void) { + // Return immediately if there is insufficient memory + if (core_malloc_failed) { + return false; + } + initFailure = InitFailures::UNKNOWN; if (_enable == 0) { if (_ahrs->get_ekf_type() == 2) { @@ -666,16 +671,16 @@ 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; + core_malloc_failed = true; gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF2: not enough memory available"); - _enable.set(0); return false; } // try to allocate from CCM RAM, fallback to Normal RAM if not available or full 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_MEM; + core_malloc_failed = true; gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF2: memory allocation failed"); return false; } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index 48fe19c63e..7cd62f01d7 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -359,6 +359,7 @@ private: uint8_t num_cores; // number of allocated cores uint8_t primary; // current primary core NavEKF2_core *core = nullptr; + bool core_malloc_failed; const AP_AHRS *_ahrs; const RangeFinder &_rng;