diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index 651cb93045..ab464d97ae 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -209,14 +209,17 @@ void NavEKF2_core::setAidingMode() bool posUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms); bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms); + // Check if external nav is being used + bool extNavUsed = (imuSampleTime_ms - lastExtNavPassTime_ms <= minTestTime_ms); + // Check if attitude drift has been constrained by a measurement source - bool attAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed; + bool attAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed || extNavUsed; // check if velocity drift has been constrained by a measurement source bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed; // check if position drift has been constrained by a measurement source - bool posAiding = posUsed || rngBcnUsed; + bool posAiding = posUsed || rngBcnUsed || extNavUsed; // Check if the loss of attitude aiding has become critical bool attAidLossCritical = false; @@ -225,6 +228,7 @@ void NavEKF2_core::setAidingMode() (imuSampleTime_ms - lastTasPassTime_ms > frontend->tiltDriftTimeMax_ms) && (imuSampleTime_ms - lastRngBcnPassTime_ms > frontend->tiltDriftTimeMax_ms) && (imuSampleTime_ms - lastPosPassTime_ms > frontend->tiltDriftTimeMax_ms) && + (imuSampleTime_ms - lastExtNavPassTime_ms > frontend->tiltDriftTimeMax_ms) && (imuSampleTime_ms - lastVelPassTime_ms > frontend->tiltDriftTimeMax_ms); } @@ -239,9 +243,11 @@ void NavEKF2_core::setAidingMode() maxLossTime_ms = frontend->posRetryTimeUseVel_ms; } posAidLossCritical = (imuSampleTime_ms - lastRngBcnPassTime_ms > maxLossTime_ms) && - (imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms); + (imuSampleTime_ms - lastExtNavPassTime_ms > maxLossTime_ms) && + (imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms); posAidLossPending = (imuSampleTime_ms - lastRngBcnPassTime_ms > (uint32_t)frontend->_gsfResetDelay) && - (imuSampleTime_ms - lastPosPassTime_ms > (uint32_t)frontend->_gsfResetDelay); + (imuSampleTime_ms - lastExtNavPassTime_ms > (uint32_t)frontend->_gsfResetDelay) && + (imuSampleTime_ms - lastPosPassTime_ms > (uint32_t)frontend->_gsfResetDelay); } if (attAidLossCritical) { @@ -335,6 +341,7 @@ void NavEKF2_core::setAidingMode() lastPosPassTime_ms = imuSampleTime_ms; lastVelPassTime_ms = imuSampleTime_ms; lastRngBcnPassTime_ms = imuSampleTime_ms; + lastExtNavPassTime_ms = imuSampleTime_ms; } break; } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index ecb100e9cd..d8e3886d46 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -1160,6 +1160,7 @@ private: ext_nav_elements extNavDataDelayed; // External nav at the fusion time horizon uint32_t extNavMeasTime_ms; // time external measurements were accepted for input to the data buffer (msec) uint32_t extNavLastPosResetTime_ms; // last time the external nav systen performed a position reset (msec) + uint32_t lastExtNavPassTime_ms; // time stamp when external nav position measurement last passed innovation consistency check (msec) bool extNavDataToFuse; // true when there is new external nav data to fuse bool extNavUsedForYaw; // true when the external nav data is also being used as a yaw observation bool extNavUsedForPos; // true when the external nav data is being used as a position reference.