diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index b754941008..259926899e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -661,16 +661,6 @@ bool NavEKF2::InitialiseFilter(void) return false; } - // allocate common intermediate variable space - core_common = (void *)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 8e3b939cda..e5b4dee6ca 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -492,11 +492,6 @@ 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_AirDataFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp index b11ede13c1..01725bac1e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp @@ -39,7 +39,6 @@ void NavEKF2_core::FuseAirspeed() float SK_TAS; Vector24 H_TAS; float VtasPred; - Vector28 Kfusion {}; // health is set bad until test passed tasHealth = false; @@ -270,7 +269,6 @@ void NavEKF2_core::FuseSideslip() Vector3f vel_rel_wind; Vector24 H_BETA; float innovBeta; - Vector28 Kfusion; // copy required states to local variable names q0 = stateStruct.quat[0]; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index 340aae16bb..fed0a152cc 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp @@ -346,7 +346,6 @@ void NavEKF2_core::FuseMagnetometer() Vector6 SK_MX; Vector6 SK_MY; Vector6 SK_MZ; - Vector28 Kfusion; hal.util->perf_end(_perf_test[1]); @@ -781,7 +780,6 @@ void NavEKF2_core::fuseEulerYaw() float measured_yaw; float H_YAW[3]; Matrix3f Tbn_zeroYaw; - Vector28 Kfusion; if (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) { // calculate observation jacobian when we are observing the first rotation in a 321 sequence @@ -1040,7 +1038,6 @@ void NavEKF2_core::FuseDeclination(float declErr) float t12 = 1.0f/t11; float H_MAG[24]; - Vector28 Kfusion {}; H_MAG[16] = -magE*t5; H_MAG[17] = magN*t5; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index 070b679e2f..4cf7ddf07e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -438,7 +438,6 @@ void NavEKF2_core::FuseVelPosNED() Vector6 R_OBS; // Measurement variances used for fusion Vector6 R_OBS_DATA_CHECKS; // Measurement variances used for data checks only float SK; - Vector28 Kfusion; // perform sequential fusion of GPS measurements. This assumes that the // errors in the different velocity and position components are diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp index d5c37fc01b..c8c9782459 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp @@ -43,7 +43,6 @@ void NavEKF2_core::FuseRngBcn() float bcn_pd; const float R_BCN = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f)); float rngPred; - Vector28 Kfusion; // health is set bad until test passed rngBcnHealth = false; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index f2a24213f0..1fa81e4d2d 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -43,12 +43,7 @@ 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), - // setup the intermediate variables shared by all cores (to save memory) - common((struct core_common *)_frontend->core_common), - KH(common->KH), - KHP(common->KHP), - nextP(common->nextP) + frontend(_frontend) { _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"); @@ -175,7 +170,9 @@ void NavEKF2_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; @@ -533,12 +530,6 @@ 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; @@ -555,6 +546,8 @@ void NavEKF2_core::UpdateFilter(bool predict) #endif hal.util->perf_begin(_perf_UpdateFilter); + fill_scratch_variables(); + // TODO - in-flight restart method //get starting time for update step diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 78ab4b6f5f..4a14e52f7e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -30,6 +30,7 @@ #include "AP_NavEKF2.h" #include #include +#include #include #include @@ -58,7 +59,7 @@ class AP_AHRS; -class NavEKF2_core +class NavEKF2_core : public NavEKF_core_common { public: // Constructor @@ -327,9 +328,6 @@ 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; @@ -339,7 +337,6 @@ private: uint8_t core_index; uint8_t imu_buffer_length; - typedef float ftype; #if MATH_CHECK_INDEXES typedef VectorN Vector2; typedef VectorN Vector3; @@ -359,7 +356,6 @@ private: typedef VectorN Vector24; typedef VectorN Vector25; typedef VectorN Vector31; - typedef VectorN Vector28; typedef VectorN,3> Matrix3; typedef VectorN,24> Matrix24; typedef VectorN,50> Matrix34_50; @@ -382,7 +378,6 @@ private: typedef ftype Vector23[23]; typedef ftype Vector24[24]; typedef ftype Vector25[25]; - typedef ftype Vector28[28]; typedef ftype Matrix3[3][3]; typedef ftype Matrix24[24][24]; typedef ftype Matrix34_50[34][50]; @@ -485,22 +480,6 @@ 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. - On SITL this structure is assumed to contain only float - variables (for the fill_nanf()) - */ - struct core_common { - Matrix24 KH; - Matrix24 KHP; - Matrix24 nextP; - } *common; - // bias estimates for the IMUs that are enabled but not being used // by this core. struct { @@ -818,8 +797,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 - 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 @@ -874,7 +851,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 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 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