ekf_check: only use gps glitch mode reversion when ekf_failsafe attem…

…pts reversion
3drobotics · Sep 14, 2016 · b930c80 · b930c80
1 parent 9b46d34
commit b930c80
Showing 1 changed file with 6 additions and 4 deletions.
diff --git 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);
+            }
         }
     }
 }