diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 0f15a51b55..303751734c 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -683,16 +683,6 @@ bool NavEKF3::InitialiseFilter(void) return false; } - // allocate common intermediate variable space - core_common = (void *)hal.util->malloc_type(NavEKF3_core::get_core_common_size(), AP_HAL::Util::MEM_FAST); - if (core_common == nullptr) { - _enable.set(0); - hal.util->free_type(core, sizeof(NavEKF3_core)*num_cores, AP_HAL::Util::MEM_FAST); - core = nullptr; - gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF3: common allocation failed"); - return false; - } - // Call constructors on all cores for (uint8_t i = 0; i < num_cores; i++) { new (&core[i]) NavEKF3_core(this); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 35c337330a..53ec71a16a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -496,11 +496,6 @@ private: // time of last lane switch uint32_t lastLaneSwitch_ms; - /* - common intermediate variables used by all cores - */ - void *core_common; - struct { uint32_t last_function_call; // last time getLastYawYawResetAngle was called bool core_changed; // true when a core change happened and hasn't been consumed, false otherwise diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 32e3638523..beac94fa11 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -20,13 +20,7 @@ NavEKF3_core::NavEKF3_core(NavEKF3 *_frontend) : _perf_TerrainOffset(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_TerrainOffset")), _perf_FuseOptFlow(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseOptFlow")), _perf_FuseBodyOdom(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseBodyOdom")), - 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) + frontend(_frontend) { _perf_test[0] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test0"); _perf_test[1] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test1"); @@ -217,7 +211,9 @@ void NavEKF3_core::InitialiseVariables() lastKnownPositionNE.zero(); prevTnb.zero(); memset(&P[0][0], 0, sizeof(P)); - memset(common, 0, sizeof(*common)); + memset(&KH[0][0], 0, sizeof(KH)); + memset(&KHP[0][0], 0, sizeof(KHP)); + memset(&nextP[0][0], 0, sizeof(nextP)); flowDataValid = false; rangeDataToFuse = false; Popt = 0.0f; @@ -573,12 +569,6 @@ void NavEKF3_core::CovarianceInit() // Update Filter States - this should be called whenever new IMU data is available void NavEKF3_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_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index b98edbfe4b..db670d8fa7 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -367,9 +367,6 @@ public: // get timing statistics structure void getTimingStatistics(struct ekf_timing &timing); - // 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 NavEKF3 *frontend; @@ -535,21 +532,6 @@ private: uint8_t type; // type specifiying Euler rotation order used, 1 = 312 (ZXY), 2 = 321 (ZYX) }; - /* - 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; - } *common; - // bias estimates for the IMUs that are enabled but not being used // by this core. struct { @@ -872,9 +854,6 @@ 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 @@ -928,7 +907,6 @@ private: bool allMagSensorsFailed; // true if all magnetometer sensors have timed out on this flight and we are no longer using magnetometer data uint32_t lastSynthYawTime_ms; // time stamp when synthetic yaw measurement was last fused to maintain covariance health (msec) uint32_t ekfStartTime_ms; // time the EKF was started (msec) - Matrix24 &nextP; // Predicted covariance matrix before addition of process noise to diagonals 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