aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF2: use parent class for intermediate static variables

Browse Source 
this makes the code faster as well as using less memory when both EK2
and EK3 are enabled
This commit is contained in:
Andrew Tridgell 2019-09-28 11:28:59 +10:00
parent 688b01c6c6
commit f7e138bddd
8 changed files with 8 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

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

5
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -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

2
libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp
View File

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

3
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

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

1
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -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

1
libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp
View File

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

19
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -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

28
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -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