Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: fail prearm on any core setup failure

Browse Source
This commit is contained in:
Mark Whitehorn 2019-11-04 07:15:28 -07:00 committed by Andrew Tridgell
parent 489ae83604
commit 23ee9eef5f
2 changed files with 8 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -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;
gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF2: allocation failed");
initFailure = InitFailures::NO_MEM;
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 (num_cores == 0) {
hal.util->free_type(core, sizeof(NavEKF2_core)*num_cores, AP_HAL::Util::MEM_FAST);
core = nullptr;
initFailure = InitFailures::NO_SETUP;
return false;
}
// if any core setup fails, free memory, zero the core pointer and abort
hal.util->free_type(core, sizeof(NavEKF2_core)*num_cores, AP_HAL::Util::MEM_FAST);
core = nullptr;
initFailure = InitFailures::NO_SETUP;
gcs().send_text(MAV_SEVERITY_WARNING, "NavEKF2: core %d setup failed", num_cores);
// don't return here, just break the loop and finish initializing
break;
return false;
}
num_cores++;
}

2
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -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;