Remove GPS glitch detection

src/main/navigation/navigation.c

@@ -3722,9 +3722,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
     if (posControl.flags.estAglStatus == EST_TRUSTED)       navFlags |= (1 << 1);
     if (posControl.flags.estPosStatus == EST_TRUSTED)       navFlags |= (1 << 2);
     if (posControl.flags.isTerrainFollowEnabled)            navFlags |= (1 << 3);
-#if defined(NAV_GPS_GLITCH_DETECTION)
-    if (isGPSGlitchDetected())                              navFlags |= (1 << 4);
-#endif
+    // naFlags |= (1 << 4); // Old NAV GPS Glitch Detection flag
     if (posControl.flags.estHeadingStatus == EST_TRUSTED)   navFlags |= (1 << 5);
 
     // Reset all navigation requests - NAV controllers will set them if necessary

src/main/navigation/navigation_pos_estimator.c

@@ -162,56 +162,6 @@ static timeUs_t getGPSDeltaTimeFilter(timeUs_t dTus)
     return dTus;                                                 // Filter failed. Set GPS Hz by measurement
 }
 
-#if defined(NAV_GPS_GLITCH_DETECTION)
-static bool detectGPSGlitch(timeUs_t currentTimeUs)
-{
-    static timeUs_t previousTime = 0;
-    static fpVector3_t lastKnownGoodPosition;
-    static fpVector3_t lastKnownGoodVelocity;
-
-    bool isGlitching = false;
-
-#ifdef USE_GPS_FIX_ESTIMATION
-    if (STATE(GPS_ESTIMATED_FIX)) {
-        //disable sanity checks in GPS estimation mode
-        //when estimated GPS fix is replaced with real fix, coordinates may jump 
-        previousTime = 0;
-        return true;
-    }
-#endif
-
-    if (previousTime == 0) {
-        isGlitching = false;
-    }
-    else {
-        fpVector3_t predictedGpsPosition;
-        float gpsDistance;
-        float dT = US2S(currentTimeUs - previousTime);
-
-        /* We predict new position based on previous GPS velocity and position */
-        predictedGpsPosition.x = lastKnownGoodPosition.x + lastKnownGoodVelocity.x * dT;
-        predictedGpsPosition.y = lastKnownGoodPosition.y + lastKnownGoodVelocity.y * dT;
-
-        /* New pos is within predefined radius of predicted pos, radius is expanded exponentially */
-        gpsDistance = calc_length_pythagorean_2D(predictedGpsPosition.x - lastKnownGoodPosition.x, predictedGpsPosition.y - lastKnownGoodPosition.y);
-        if (gpsDistance <= (INAV_GPS_GLITCH_RADIUS + 0.5f * INAV_GPS_GLITCH_ACCEL * dT * dT)) {
-            isGlitching = false;
-        }
-        else {
-            isGlitching = true;
-        }
-    }
-
-    if (!isGlitching) {
-        previousTime = currentTimeUs;
-        lastKnownGoodPosition = posEstimator.gps.pos;
-        lastKnownGoodVelocity = posEstimator.gps.vel;
-    }
-
-    return isGlitching;
-}
-#endif
-
 /**
  * Update GPS topic
  *  Function is called on each GPS update
@@ -295,19 +245,6 @@ void onNewGPSData(void)
                     posEstimator.gps.vel.z = (posEstimator.gps.vel.z + (gpsSol.llh.alt - previousAlt) / dT) / 2.0f;
                 }
 
-#if defined(NAV_GPS_GLITCH_DETECTION)
-                /* GPS glitch protection. We have local coordinates and local velocity for current GPS update. Check if they are sane */
-                if (detectGPSGlitch(currentTimeUs)) {
-                    posEstimator.gps.glitchRecovery = false;
-                    posEstimator.gps.glitchDetected = true;
-                }
-                else {
-                    /* Store previous glitch flag in glitchRecovery to indicate a valid reading after a glitch */
-                    posEstimator.gps.glitchRecovery = posEstimator.gps.glitchDetected;
-                    posEstimator.gps.glitchDetected = false;
-                }
-#endif
-
                 /* FIXME: use HDOP/VDOP */
                 if (gpsSol.flags.validEPE) {
                     posEstimator.gps.eph = gpsSol.eph;
@@ -895,13 +832,6 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
     }
 }
 
-#if defined(NAV_GPS_GLITCH_DETECTION)
-bool isGPSGlitchDetected(void)
-{
-    return posEstimator.gps.glitchDetected;
-}
-#endif
-
 float getEstimatedAglPosition(void) {
     return posEstimator.est.aglAlt;
 }

src/main/navigation/navigation_pos_estimator_private.h

@@ -65,10 +65,6 @@ typedef struct {
 
 typedef struct {
     timeUs_t    lastUpdateTime; // Last update time (us)
-#if defined(NAV_GPS_GLITCH_DETECTION)
-    bool        glitchDetected;
-    bool        glitchRecovery;
-#endif
     fpVector3_t pos;            // GPS position in NEU coordinate system (cm)
     fpVector3_t vel;            // GPS velocity (cms)
     float       eph;

src/main/sensors/compass.c

@@ -485,4 +485,5 @@ void compassUpdate(timeUs_t currentTimeUs)
 
     magUpdatedAtLeastOnce = true;
 }
+
 #endif