Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 15:53:56 -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 // check if there is enough memory to create the EKF cores
if (hal.util->available_memory() < sizeof(NavEKF2_core)*num_cores + 4096) { if (hal.util->available_memory() < sizeof(NavEKF2_core)*num_cores + 4096) {
initFailure = InitFailures::NO_MEM; 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); _enable.set(0);
return false; 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); core = (NavEKF2_core*)hal.util->malloc_type(sizeof(NavEKF2_core)*num_cores, AP_HAL::Util::MEM_FAST);
if (core == nullptr) { if (core == nullptr) {
_enable.set(0); _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; return false;
} }
@ -690,16 +690,12 @@ bool NavEKF2::InitialiseFilter(void)
for (uint8_t i=0; i<7; i++) { for (uint8_t i=0; i<7; i++) {
if (_imuMask & (1U<<i)) { if (_imuMask & (1U<<i)) {
if(!core[num_cores].setup_core(i, num_cores)) { 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); 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++; num_cores++;
} }

2
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

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