mirror of https://github.com/ArduPilot/ardupilot
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:
parent
6ffffff8c6
commit
164813bcc2
|
@ -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];
|
||||
|
|
|
@ -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,7 @@ 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
|
||||
|
@ -1038,6 +1040,7 @@ 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;
|
||||
|
|
|
@ -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];
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -46,15 +46,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");
|
||||
|
|
|
@ -492,15 +492,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
|
||||
|
@ -820,7 +814,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
|
||||
|
@ -878,11 +871,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
|
||||
|
|
Loading…
Reference in New Issue