diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 259926899e..76e339bade 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -661,6 +661,16 @@ bool NavEKF2::InitialiseFilter(void) return false; } + // allocate common intermediate variable space + core_common = (struct CoreCommon *)hal.util->malloc_type(NavEKF2_core::get_core_common_size(), AP_HAL::Util::MEM_FAST); + if (core_common == nullptr) { + _enable.set(0); + hal.util->free_type(core, sizeof(NavEKF2_core)*num_cores, AP_HAL::Util::MEM_FAST); + core = nullptr; + gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF2: common allocation failed"); + return false; + } + //Call Constructors on all cores for (uint8_t i = 0; i < num_cores; i++) { new (&core[i]) NavEKF2_core(this); diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index e5b4dee6ca..8e3b939cda 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -492,6 +492,11 @@ private: // time of last lane switch uint32_t lastLaneSwitch_ms; + /* + common intermediate variables used by all cores + */ + void *core_common; + // update the yaw reset data to capture changes due to a lane switch // new_primary - index of the ekf instance that we are about to switch to as the primary // old_primary - index of the ekf instance that we are currently using as the primary diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 1fa81e4d2d..351eecadf6 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -43,7 +43,18 @@ NavEKF2_core::NavEKF2_core(NavEKF2 *_frontend) : _perf_FuseSideslip(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseSideslip")), _perf_TerrainOffset(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_TerrainOffset")), _perf_FuseOptFlow(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseOptFlow")), - frontend(_frontend) + frontend(_frontend), + // setup the intermediate variables shared by all cores (to save memory) + common((struct core_common *)_frontend->core_common), + Kfusion(common->Kfusion), + KH(common->KH), + KHP(common->KHP), + nextP(common->nextP), + processNoise(common->processNoise), + SF(common->SF), + SG(common->SG), + SQ(common->SQ), + SPP(common->SPP) { _perf_test[0] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test0"); _perf_test[1] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test1"); @@ -170,9 +181,7 @@ void NavEKF2_core::InitialiseVariables() lastKnownPositionNE.zero(); prevTnb.zero(); memset(&P[0][0], 0, sizeof(P)); - memset(&KH[0][0], 0, sizeof(KH)); - memset(&KHP[0][0], 0, sizeof(KHP)); - memset(&nextP[0][0], 0, sizeof(nextP)); + memset(common, 0, sizeof(*common)); flowDataValid = false; rangeDataToFuse = false; Popt = 0.0f; @@ -530,6 +539,12 @@ void NavEKF2_core::CovarianceInit() // Update Filter States - this should be called whenever new IMU data is available void NavEKF2_core::UpdateFilter(bool predict) { +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + // fill the common variables with NaN, so we catch any cases in + // SITL where they are used without initialisation + fill_nanf((float *)common, sizeof(*common)/sizeof(float)); +#endif + // Set the flag to indicate to the filter that the front-end has given permission for a new state prediction cycle to be started startPredictEnabled = predict; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 4a14e52f7e..94e6f6960e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -328,6 +328,9 @@ public: // return true when external nav data is also being used as a yaw observation bool isExtNavUsedForYaw(void); + // return size of core_common for use in frontend allocation + static uint32_t get_core_common_size(void) { return sizeof(core_common); } + private: // Reference to the global EKF frontend for parameters NavEKF2 *frontend; @@ -480,6 +483,26 @@ private: bool posReset; // true when the position measurement has been reset }; + /* + common intermediate variables used by all cores. These save a + lot memory by avoiding allocating these arrays on every core + Having these as stack variables would save even more memory, but + would give us very high stack usage in some functions, which + poses a risk of stack overflow until we have infrastructure in + place to calculate maximum stack usage using static analysis. + */ + struct core_common { + Vector28 Kfusion; + Matrix24 KH; + Matrix24 KHP; + Matrix24 nextP; + Vector24 processNoise; + Vector25 SF; + Vector5 SG; + Vector8 SQ; + Vector23 SPP; + } *common; + // bias estimates for the IMUs that are enabled but not being used // by this core. struct { @@ -797,6 +820,9 @@ private: bool badIMUdata; // boolean true if the bad IMU data is detected float gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts + Vector28 &Kfusion; // Kalman gain vector + Matrix24 &KH; // intermediate result used for covariance updates + Matrix24 &KHP; // intermediate result used for covariance updates Matrix24 P; // covariance matrix imu_ring_buffer_t storedIMU; // IMU data buffer obs_ring_buffer_t storedGPS; // GPS data buffer @@ -851,6 +877,12 @@ private: bool allMagSensorsFailed; // true if all magnetometer sensors have timed out on this flight and we are no longer using magnetometer data uint32_t lastYawTime_ms; // time stamp when yaw observation was last fused (msec) uint32_t ekfStartTime_ms; // time the EKF was started (msec) + Matrix24 &nextP; // Predicted covariance matrix before addition of process noise to diagonals + Vector24 &processNoise; // process noise added to diagonals of predicted covariance matrix + Vector25 &SF; // intermediate variables used to calculate predicted covariance matrix + Vector5 &SG; // intermediate variables used to calculate predicted covariance matrix + Vector8 &SQ; // intermediate variables used to calculate predicted covariance matrix + Vector23 &SPP; // intermediate variables used to calculate predicted covariance matrix Vector2f lastKnownPositionNE; // last known position uint32_t lastDecayTime_ms; // time of last decay of GPS position offset float velTestRatio; // sum of squares of GPS velocity innovation divided by fail threshold