diff --git a/ArduCopter/ekf_check.pde b/ArduCopter/ekf_check.pde
index e0a74c934a..a1d60f34eb 100644
--- a/ArduCopter/ekf_check.pde
+++ b/ArduCopter/ekf_check.pde
@@ -176,12 +176,14 @@ static void failsafe_ekf_off_event(void)
     Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_RESOLVED);
     
     if (ekf_check_switch_mode_on_resolve) {
-        if (failsafe.gps_glitch) {
-            gps_glitch_switch_mode_on_resolve = true;
-        } else if (!mode_requires_RC(ekf_check_mode_before_fs_on)) {
+        if (!mode_requires_RC(ekf_check_mode_before_fs_on)) {
             set_mode_RTL_or_land_with_pause();
         } else if (mode_requires_GPS(ekf_check_mode_before_fs_on)) {
-            set_mode(LOITER);
+            if (failsafe.gps_glitch) {
+                gps_glitch_switch_mode_on_resolve = true;
+            } else {
+                set_mode(LOITER);
+            }
         }
     }
 }