AP_NavEKF3: improve ext nav glitch handling

replaces extNavTimeout with posTimeout
replaces lastExtNavPassTime_ms with lastPosPassTime_ms

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -247,21 +247,18 @@ void NavEKF3_core::setAidingMode()
             // Check if range beacon data is being used
             bool rngBcnUsed = (imuSampleTime_ms - lastRngBcnPassTime_ms <= minTestTime_ms);
 
-            // Check if GPS is being used
+            // Check if GPS or external nav is being used
-            bool gpsPosUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms);
+            bool posUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms);
             bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms);
 
-            // Check if external nav position is being used
-            bool extNavUsed = (imuSampleTime_ms - lastExtNavPassTime_ms <= minTestTime_ms);
-
             // Check if attitude drift has been constrained by a measurement source
-            bool attAiding = gpsPosUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed || bodyOdmUsed || extNavUsed;
+            bool attAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed || bodyOdmUsed;
 
             // check if velocity drift has been constrained by a measurement source
             bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed || bodyOdmUsed;
 
             // check if position drift has been constrained by a measurement source
-            bool posAiding = gpsPosUsed || rngBcnUsed || extNavUsed;
+            bool posAiding = posUsed || rngBcnUsed;
 
             // Check if the loss of attitude aiding has become critical
             bool attAidLossCritical = false;
@@ -270,7 +267,6 @@ void NavEKF3_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);
             }
 
@@ -284,7 +280,6 @@ void NavEKF3_core::setAidingMode()
                     maxLossTime_ms = frontend->posRetryTimeUseVel_ms;
                 }
                 posAidLossCritical = (imuSampleTime_ms - lastRngBcnPassTime_ms > maxLossTime_ms) &&
-                                     (imuSampleTime_ms - lastExtNavPassTime_ms > maxLossTime_ms) &&
                                      (imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms);
             }
 
@@ -296,14 +291,12 @@ void NavEKF3_core::setAidingMode()
                 rngBcnTimeout = true;
                 tasTimeout = true;
                 gpsNotAvailable = true;
-                extNavTimeout = true;
              } else if (posAidLossCritical) {
                 // if the loss of position is critical, declare all sources of position aiding as being timed out
                 posTimeout = true;
                 velTimeout = true;
                 rngBcnTimeout = true;
                 gpsNotAvailable = true;
-                extNavTimeout = true;
 
             }
             break;
@@ -385,7 +378,6 @@ void NavEKF3_core::setAidingMode()
             lastPosPassTime_ms = imuSampleTime_ms;
             lastVelPassTime_ms = imuSampleTime_ms;
             lastRngBcnPassTime_ms = imuSampleTime_ms;
-            lastExtNavPassTime_ms = imuSampleTime_ms;
             break;
         }
 

libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@@ -128,8 +128,8 @@ void NavEKF3_core::ResetPosition(void)
             // set the variances as received from external nav system data
             P[7][7] = P[8][8] = sq(extNavDataDelayed.posErr);
             // clear the timeout flags and counters
-            extNavTimeout = false;
+            posTimeout = false;
-            lastExtNavPassTime_ms = imuSampleTime_ms;
+            lastPosPassTime_ms = imuSampleTime_ms;
         }
     }
     for (uint8_t i=0; i<imu_buffer_length; i++) {

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -398,10 +398,8 @@ void NavEKF3_core::InitialiseVariables()
     extNavDataDelayed = {};
     extNavMeasTime_ms = 0;
     extNavLastPosResetTime_ms = 0;
-    lastExtNavPassTime_ms = 0;
     extNavDataToFuse = false;
     extNavUsedForPos = false;
-    extNavTimeout = false;
 
     // zero data buffers
     storedIMU.reset();

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -1334,10 +1334,8 @@ 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 extNavUsedForPos;              // true when the external nav data is being used as a position reference.
-    bool extNavTimeout;                 // true if external nav measurements have failed innovation consistency checks for too long
 
     // flags indicating severe numerical errors in innovation variance calculation for different fusion operations
     struct {