AP_NavEKF3: allocate from MEM_FAST region

This commit is contained in:
bugobliterator 2018-01-08 15:13:52 +05:30 committed by Andrew Tridgell
parent b1213a522d
commit ea2a880d8a
1 changed files with 9 additions and 5 deletions

View File

@ -5,6 +5,7 @@
#include <AP_Vehicle/AP_Vehicle.h>
#include <GCS_MAVLink/GCS.h>
#include <DataFlash/DataFlash.h>
#include <new>
/*
parameter defaults for different types of vehicle. The
@ -697,15 +698,18 @@ bool NavEKF3::InitialiseFilter(void)
_enable.set(0);
return false;
}
// create the cores
core = new NavEKF3_core[num_cores];
if (core == nullptr) {
//try to allocate from CCM RAM, fallback to Normal RAM if not available or full
core = (NavEKF3_core*)hal.util->malloc_type(sizeof(NavEKF3_core)*num_cores, AP_HAL::Util::MEM_FAST);
if (core == nullptr) {
_enable.set(0);
gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF3: allocation failed");
return false;
}
for (uint8_t i = 0; i < num_cores; i++) {
//Call Constructors
new (&core[i]) NavEKF3_core();
}
}
// Set up any cores that have been created