AP_NavEKF2: Clean up GPS timeout logic

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -558,37 +558,34 @@ void NavEKF2_core::readGpsData()
     // If we haven't received GPS data for a while and we are using it for aiding, then declare the position and velocity data as being timed out
     if (imuSampleTime_ms - lastTimeGpsReceived_ms > gpsFailTimeout_ms) {
 
-        // Let other processes know that GPS i snota vailable and that a timeout has occurred
+        // Let other processes know that GPS is not available and that a timeout has occurred
         posTimeout = true;
         velTimeout = true;
         gpsNotAvailable = true;
 
-        // If we are currently reliying on GPS for navigation, then we need to switch to a non-GPS mode of operation
-        if (PV_AidingMode == AID_ABSOLUTE) {
-
-            // If we don't have airspeed or sideslip assumption or optical flow to constrain drift, then go into constant position mode.
-            // If we can do optical flow nav (valid flow data and height above ground estimate), then go into flow nav mode.
-            if (!useAirspeed() && !assume_zero_sideslip()) {
-                if (optFlowBackupAvailable) {
-                    // we can do optical flow only nav
-                    frontend._fusionModeGPS = 3;
-                    PV_AidingMode = AID_RELATIVE;
-                } else {
-                    // store the current position
-                    lastKnownPositionNE.x = stateStruct.position.x;
-                    lastKnownPositionNE.y = stateStruct.position.y;
+        // If we are totally reliant on GPS for navigation, then we need to switch to a non-GPS mode of operation
+        // If we don't have airspeed or sideslip assumption or optical flow to constrain drift, then go into constant position mode.
+        // If we can do optical flow nav (valid flow data and height above ground estimate), then go into flow nav mode.
+        if (PV_AidingMode == AID_ABSOLUTE && !useAirspeed() && !assume_zero_sideslip()) {
+            if (optFlowBackupAvailable) {
+                // we can do optical flow only nav
+                frontend._fusionModeGPS = 3;
+                PV_AidingMode = AID_RELATIVE;
+            } else {
+                // store the current position
+                lastKnownPositionNE.x = stateStruct.position.x;
+                lastKnownPositionNE.y = stateStruct.position.y;
 
-                    // put the filter into constant position mode
-                    PV_AidingMode = AID_NONE;
+                // put the filter into constant position mode
+                PV_AidingMode = AID_NONE;
 
-                    // Reset the velocity and position states
-                    ResetVelocity();
-                    ResetPosition();
+                // Reset the velocity and position states
+                ResetVelocity();
+                ResetPosition();
 
-                    // Reset the normalised innovation to avoid false failing the bad position fusion test
-                    velTestRatio = 0.0f;
-                    posTestRatio = 0.0f;
-                }
+                // Reset the normalised innovation to avoid false failing the bad position fusion test
+                velTestRatio = 0.0f;
+                posTestRatio = 0.0f;
             }
         }
     }