mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF2: clean up init failure handling
only attempt to allocate memory once
This commit is contained in:
parent
feead0a42c
commit
5f6b3a1f5b
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
Loading…
Reference in New Issue