Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: handle core setup failures

Browse Source
This commit is contained in:
Mark Whitehorn 2019-10-31 21:50:22 -06:00 committed by Andrew Tridgell
parent 7c6630e216
commit 489ae83604
2 changed files with 23 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -646,16 +646,6 @@ bool NavEKF2::InitialiseFilter(void)
if (core == nullptr) { if (core == nullptr) {
// this prevents dereferencing a null _ahrs pointer in NavEKF2_core::getEulerAngles
if (ins.get_accel_count() == 0) {
initFailure = InitFailures::NO_IMUS;
return false;
}
if (_imuMask == 0) {
initFailure = InitFailures::NO_MASK;
return false;
}
// don't allow more filters than IMUs // don't allow more filters than IMUs
uint8_t mask = (1U<<ins.get_accel_count())-1; uint8_t mask = (1U<<ins.get_accel_count())-1;
_imuMask.set(_imuMask.get() & mask); _imuMask.set(_imuMask.get() & mask);
@ -663,6 +653,16 @@ bool NavEKF2::InitialiseFilter(void)
// count IMUs from mask // count IMUs from mask
num_cores = __builtin_popcount(_imuMask); num_cores = __builtin_popcount(_imuMask);
// abort if num_cores is zero
if (num_cores == 0) {
if (ins.get_accel_count() == 0) {
initFailure = InitFailures::NO_IMUS;
} else {
initFailure = InitFailures::NO_MASK;
}
return false;
}
// 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;
@ -690,7 +690,16 @@ 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)) {
return false; // 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;
}
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;
} }
num_cores++; num_cores++;
} }

4
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -500,6 +500,7 @@ private:
NO_MASK, NO_MASK,
NO_MEM, NO_MEM,
NO_ALLOC, NO_ALLOC,
NO_SETUP,
NUM_INIT_FAILURES NUM_INIT_FAILURES
}; };
// initialization failure reasons // initialization failure reasons
@ -509,7 +510,8 @@ 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: memory allocation failed",
"EKF2: core setup failed"
}; };
InitFailures initFailure; InitFailures initFailure;