AP_NavEKF2: use parent class for intermediate static variables

this makes the code faster as well as using less memory when both EK2
and EK3 are enabled

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);

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

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];

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;

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

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;

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),
+    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)
 {
     _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

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -30,6 +30,7 @@
 #include "AP_NavEKF2.h"
 #include <stdio.h>
 #include <AP_Math/vectorN.h>
+#include <AP_NavEKF/AP_NavEKF_core_common.h>
 #include <AP_NavEKF2/AP_NavEKF2_Buffer.h>
 #include <AP_InertialSensor/AP_InertialSensor.h>
 
@@ -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<ftype,2> Vector2;
     typedef VectorN<ftype,3> Vector3;
@@ -359,7 +356,6 @@ private:
     typedef VectorN<ftype,24> Vector24;
     typedef VectorN<ftype,25> Vector25;
     typedef VectorN<ftype,31> Vector31;
-    typedef VectorN<ftype,28> Vector28;
     typedef VectorN<VectorN<ftype,3>,3> Matrix3;
     typedef VectorN<VectorN<ftype,24>,24> Matrix24;
     typedef VectorN<VectorN<ftype,34>,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<imu_elements> storedIMU;      // IMU data buffer
     obs_ring_buffer_t<gps_elements> 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