AP_NavEKF3: handle core setup failure

ensure num_cores is left as zero so that calls such as Log_Write don't
de-reference nullptr
This commit is contained in:
Andrew Tridgell 2023-05-02 10:39:16 +10:00
parent 5a5c9d10a9
commit 52cb59e61d

View File

@ -803,6 +803,7 @@ bool NavEKF3::InitialiseFilter(void)
if (AP::dal().available_memory() < sizeof(NavEKF3_core)*num_cores + 4096) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF3 not enough memory");
_enable.set(0);
num_cores = 0;
return false;
}
@ -810,6 +811,7 @@ bool NavEKF3::InitialiseFilter(void)
core = (NavEKF3_core*)AP::dal().malloc_type(sizeof(NavEKF3_core)*num_cores, AP::dal().MEM_FAST);
if (core == nullptr) {
_enable.set(0);
num_cores = 0;
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF3 allocation failed");
return false;
}