diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 9316b36efe..8fc369c392 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -487,7 +487,7 @@ bool NavEKF3_core::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::Sour switch (source) { case AP_NavEKF_Source::SourceXY::GPS: // check for timeouts - if (dal.millis() - gpsVelInnovTime_ms > 500) { + if (dal.millis() - gpsRetrieveTime_ms > 500) { return false; } innovations = gpsVelInnov.tofloat(); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index cd45746cdf..8f73f8ae02 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -500,8 +500,8 @@ void NavEKF3_core::SelectVelPosFusion() CorrectGPSForAntennaOffset(gpsDataDelayed); // calculate innovations and variances for reporting purposes only CalculateVelInnovationsAndVariances(gpsDataDelayed.vel, frontend->_gpsHorizVelNoise, frontend->gpsNEVelVarAccScale, gpsVelInnov, gpsVelVarInnov); - // record time innovations were calculated (for timeout checks) - gpsVelInnovTime_ms = dal.millis(); + // record time GPS data was retrieved from the buffer (for timeout checks) + gpsRetrieveTime_ms = dal.millis(); } // detect position source changes. Trigger position reset if position source is valid diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index e1b8912c73..c7bef11e76 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -1259,7 +1259,7 @@ private: bool gpsAccuracyGoodForAltitude; // true when the GPS accuracy is considered to be good enough to use it as an altitude source. Vector3F gpsVelInnov; // gps velocity innovations Vector3F gpsVelVarInnov; // gps velocity innovation variances - uint32_t gpsVelInnovTime_ms; // system time that gps velocity innovations were recorded (to detect timeouts) + uint32_t gpsRetrieveTime_ms; // system time that GPS data was retrieved from the buffer (to detect timeouts) // variables added for optical flow fusion EKF_obs_buffer_t storedOF; // OF data buffer