diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp index a6b71e4a03..62c882a63c 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp @@ -241,7 +241,7 @@ void NavEKF3_core::SelectBetaDragFusion() bool f_timeTrigger = ((imuSampleTime_ms - prevBetaDragStep_ms) >= frontend->betaAvg_ms); // use of air data to constrain drift is necessary if we have limited sensor data or are doing inertial dead reckoning - bool is_dead_reckoning = ((imuSampleTime_ms - lastPosPassTime_ms) > frontend->deadReckonDeclare_ms) && ((imuSampleTime_ms - lastVelPassTime_ms) > frontend->deadReckonDeclare_ms); + bool is_dead_reckoning = ((imuSampleTime_ms - lastGpsPosPassTime_ms) > frontend->deadReckonDeclare_ms) && ((imuSampleTime_ms - lastVelPassTime_ms) > frontend->deadReckonDeclare_ms); const bool noYawSensor = !use_compass() && !using_noncompass_for_yaw(); const bool f_required = (noYawSensor && (frontend->_betaMask & (1<<1))) || is_dead_reckoning; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 6125d26d93..3eea5d16d2 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -336,7 +336,7 @@ void NavEKF3_core::setAidingMode() #endif // Check if GPS or external nav is being used - bool posUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms); + bool posUsed = (imuSampleTime_ms - lastGpsPosPassTime_ms <= minTestTime_ms); bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms); // Check if attitude drift has been constrained by a measurement source @@ -365,7 +365,7 @@ void NavEKF3_core::setAidingMode() #if EK3_FEATURE_BEACON_FUSION (imuSampleTime_ms - rngBcn.lastPassTime_ms > frontend->tiltDriftTimeMax_ms) && #endif - (imuSampleTime_ms - lastPosPassTime_ms > frontend->tiltDriftTimeMax_ms) && + (imuSampleTime_ms - lastGpsPosPassTime_ms > frontend->tiltDriftTimeMax_ms) && (imuSampleTime_ms - lastVelPassTime_ms > frontend->tiltDriftTimeMax_ms); } @@ -382,7 +382,7 @@ void NavEKF3_core::setAidingMode() #if EK3_FEATURE_BEACON_FUSION (imuSampleTime_ms - rngBcn.lastPassTime_ms > maxLossTime_ms) && #endif - (imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms); + (imuSampleTime_ms - lastGpsPosPassTime_ms > maxLossTime_ms); } if (attAidLossCritical) { @@ -488,7 +488,7 @@ void NavEKF3_core::setAidingMode() velTimeout = false; // reset the last fusion accepted times to prevent unwanted activation of timeout logic - lastPosPassTime_ms = imuSampleTime_ms; + lastGpsPosPassTime_ms = imuSampleTime_ms; lastVelPassTime_ms = imuSampleTime_ms; #if EK3_FEATURE_BEACON_FUSION rngBcn.lastPassTime_ms = imuSampleTime_ms; @@ -786,7 +786,7 @@ void NavEKF3_core::updateFilterStatus(void) status.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected status.flags.takeoff = dal.get_takeoff_expected(); // The EKF has been told to expect takeoff is in a ground effect mitigation mode and has started the EKF-GSF yaw estimator status.flags.touchdown = dal.get_touchdown_expected(); // The EKF has been told to detect touchdown and is in a ground effect mitigation mode - status.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE); + status.flags.using_gps = ((imuSampleTime_ms - lastGpsPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE); status.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS); // GPS glitching is affecting navigation accuracy status.flags.gps_quality_good = gpsGoodToAlign; // for reporting purposes we report rejecting airspeed after 3s of not fusing when we want to fuse the data diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index d2887c8fdc..cd45746cdf 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -141,7 +141,7 @@ void NavEKF3_core::ResetPosition(resetDataSource posResetSource) // clear the timeout flags and counters posTimeout = false; - lastPosPassTime_ms = imuSampleTime_ms; + lastGpsPosPassTime_ms = imuSampleTime_ms; } #if EK3_FEATURE_POSITION_RESET @@ -150,7 +150,7 @@ void NavEKF3_core::ResetPosition(resetDataSource posResetSource) // Returns true if the set was successful bool NavEKF3_core::setLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms) { - if ((imuSampleTime_ms - lastPosPassTime_ms) < frontend->deadReckonDeclare_ms || + if ((imuSampleTime_ms - lastGpsPosPassTime_ms) < frontend->deadReckonDeclare_ms || (PV_AidingMode == AID_NONE) || !validOrigin) { return false; @@ -775,17 +775,17 @@ void NavEKF3_core::FuseVelPosNED() posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2; if (posTestRatio < 1.0f || (PV_AidingMode == AID_NONE)) { posCheckPassed = true; - lastPosPassTime_ms = imuSampleTime_ms; + lastGpsPosPassTime_ms = imuSampleTime_ms; } else if ((frontend->_gpsGlitchRadiusMax <= 0) && (PV_AidingMode != AID_NONE)) { // Handle the special case where the glitch radius parameter has been set to a non-positive number. // The innovation variance is increased to limit the state update to an amount corresponding // to a test ratio of 1. posCheckPassed = true; - lastPosPassTime_ms = imuSampleTime_ms; + lastGpsPosPassTime_ms = imuSampleTime_ms; varInnovVelPos[3] *= posTestRatio; varInnovVelPos[4] *= posTestRatio; posCheckPassed = true; - lastPosPassTime_ms = imuSampleTime_ms; + lastGpsPosPassTime_ms = imuSampleTime_ms; } // Use position data if healthy or timed out or bad IMU data @@ -856,7 +856,7 @@ void NavEKF3_core::FuseVelPosNED() // The innovation variance is increased to limit the state update to an amount corresponding // to a test ratio of 1. posCheckPassed = true; - lastPosPassTime_ms = imuSampleTime_ms; + lastGpsPosPassTime_ms = imuSampleTime_ms; for (uint8_t i = 0; i<=imax; i++) { varInnovVelPos[i] *= velTestRatio; } @@ -904,7 +904,7 @@ void NavEKF3_core::FuseVelPosNED() // The innovation variance is increased to limit the state update to an amount corresponding // to a test ratio of 1. posCheckPassed = true; - lastPosPassTime_ms = imuSampleTime_ms; + lastGpsPosPassTime_ms = imuSampleTime_ms; varInnovVelPos[5] *= hgtTestRatio; hgtCheckPassed = true; lastHgtPassTime_ms = imuSampleTime_ms; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 4e3f1b08d0..7d5e685afa 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -203,7 +203,7 @@ void NavEKF3_core::InitialiseVariables() prevBetaDragStep_ms = imuSampleTime_ms; lastBaroReceived_ms = imuSampleTime_ms; lastVelPassTime_ms = 0; - lastPosPassTime_ms = 0; + lastGpsPosPassTime_ms = 0; lastHgtPassTime_ms = 0; lastTasPassTime_ms = 0; lastSynthYawTime_ms = 0; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 521b76443c..e1b8912c73 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -1114,7 +1114,7 @@ private: uint32_t lastBaroReceived_ms; // time last time we received baro height data uint16_t hgtRetryTime_ms; // time allowed without use of height measurements before a height timeout is declared uint32_t lastVelPassTime_ms; // time stamp when GPS velocity measurement last passed innovation consistency check (msec) - uint32_t lastPosPassTime_ms; // time stamp when GPS position measurement last passed innovation consistency check (msec) + uint32_t lastGpsPosPassTime_ms; // time stamp when GPS position measurement last passed innovation consistency check (msec) uint32_t lastHgtPassTime_ms; // time stamp when height measurement last passed innovation consistency check (msec) uint32_t lastTasPassTime_ms; // time stamp when airspeed measurement last passed innovation consistency check (msec) uint32_t lastTasFailTime_ms; // time stamp when airspeed measurement last failed innovation consistency check (msec)