Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: moved some of the intermediate vars to the stack 

this keeps stack frames below 1k, while giving faster access to the
variables and saving more memory
This commit is contained in:
Andrew Tridgell 2019-09-21 16:05:22 +10:00
parent 96c6544997
commit 255981c60c
7 changed files with 16 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp
View File

@ -39,6 +39,7 @@ void NavEKF2_core::FuseAirspeed()
float SK_TAS;
Vector24 H_TAS;
float VtasPred;
Vector28 Kfusion;
// health is set bad until test passed
tasHealth = false;
@ -269,6 +270,7 @@ 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];

5
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -346,6 +346,7 @@ void NavEKF2_core::FuseMagnetometer()
Vector6 SK_MX;
Vector6 SK_MY;
Vector6 SK_MZ;
Vector28 Kfusion;
hal.util->perf_end(_perf_test[1]);
@ -780,6 +781,8 @@ 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
float t2 = q0*q0;
@ -1037,6 +1040,8 @@ 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_OptFlowFusion.cpp
View File

@ -285,6 +285,7 @@ void NavEKF2_core::FuseOptFlow()
Vector3f relVelSensor;
Vector14 SH_LOS;
Vector2 losPred;
Vector28 Kfusion;
// Copy required states to local variable names
float q0 = stateStruct.quat[0];

1
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -438,6 +438,7 @@ 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,6 +43,7 @@ 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;

13
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -32,15 +32,9 @@ NavEKF2_core::NavEKF2_core(NavEKF2 *_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)
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");
@ -882,6 +876,10 @@ void NavEKF2_core::CovariancePrediction()
float day_s; // Y axis delta angle measurement scale factor
float daz_s; // Z axis delta angle measurement scale factor
float dvz_b; // Z axis delta velocity measurement bias (rad)
Vector25 SF;
Vector5 SG;
Vector8 SQ;
Vector24 processNoise;
// calculate covariance prediction process noise
// use filtered height rate to increase wind process noise when climbing or descending
@ -979,6 +977,7 @@ void NavEKF2_core::CovariancePrediction()
SQ[6] = 2*q1*q2;
SQ[7] = SG[4];
Vector23 SPP;
SPP[0] = SF[17]*(2*q0*q1 + 2*q2*q3) + SF[18]*(2*q0*q2 - 2*q1*q3);
SPP[1] = SF[18]*(2*q0*q2 + 2*q1*q3) + SF[16]*(SF[24] - 2*q0*q3);
SPP[2] = 2*q3*SF[8] + 2*q1*SF[11] - 2*q0*SF[14] - 2*q2*SF[13];

12
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -494,15 +494,9 @@ private:
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
@ -822,7 +816,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
@ -880,11 +873,6 @@ private:
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