Copter: add GPS glitch failsafe

ArduCopter/ArduCopter.pde

@@ -411,6 +411,7 @@ static struct {
     uint8_t battery             : 1; // 2   // A status flag for the battery failsafe
     uint8_t gcs                 : 1; // 4   // A status flag for the ground station failsafe
     uint8_t ekf                 : 1; // 5   // true if ekf failsafe has occurred
+    uint8_t gps_glitch          : 1; // 6   // true if gps glitch failsafe has occurred
 
     int8_t radio_counter;                  // number of iterations with throttle below throttle_fs_value
 
@@ -920,6 +921,8 @@ static void fast_loop()
     update_land_detector();
 
     update_motor_fail_detector();
+
+    gps_glitch_update();
 }
 
 // rc_loops - reads user input from transmitter/receiver

ArduCopter/control_auto.pde

@@ -20,6 +20,10 @@
 // auto_init - initialise auto controller
 static bool auto_init(bool ignore_checks)
 {
+    if (!ignore_checks && failsafe.gps_glitch) {
+        return false;
+    }
+
     if ((position_ok() && mission.num_commands() > 1) || ignore_checks) {
         auto_mode = Auto_Loiter;
 

ArduCopter/control_circle.pde

@@ -7,6 +7,10 @@
 // circle_init - initialise circle controller flight mode
 static bool circle_init(bool ignore_checks)
 {
+    if (!ignore_checks && failsafe.gps_glitch) {
+        return false;
+    }
+
     if (position_ok() || ignore_checks) {
         circle_pilot_yaw_override = false;
 

ArduCopter/control_drift.pde

@@ -29,6 +29,10 @@
 // drift_init - initialise drift controller
 static bool drift_init(bool ignore_checks)
 {
+    if (!ignore_checks && failsafe.gps_glitch) {
+        return false;
+    }
+
     if (position_ok() || ignore_checks) {
         return true;
     }else{

ArduCopter/control_guided.pde

@@ -26,6 +26,10 @@ struct Guided_Limit {
 // guided_init - initialise guided controller
 static bool guided_init(bool ignore_checks)
 {
+    if (!ignore_checks && failsafe.gps_glitch) {
+        return false;
+    }
+
     if (position_ok() || ignore_checks) {
         // initialise yaw
         set_auto_yaw_mode(get_default_auto_yaw_mode(false));

ArduCopter/control_land.pde

@@ -9,7 +9,7 @@ static bool land_pause;
 static bool land_init(bool ignore_checks)
 {
     // check if we have GPS and decide which LAND we're going to do
-    land_with_gps = position_ok();
+    land_with_gps = position_ok() && !failsafe.gps_glitch;
     if (land_with_gps) {
         // set target to stopping point
         Vector3f stopping_point;

ArduCopter/control_loiter.pde

@@ -7,6 +7,10 @@
 // loiter_init - initialise loiter controller
 static bool loiter_init(bool ignore_checks)
 {
+    if (!ignore_checks && failsafe.gps_glitch) {
+        return false;
+    }
+
     if (position_ok() || optflow_position_ok() || ignore_checks) {
 
         // set target to current position

ArduCopter/control_poshold.pde

@@ -96,6 +96,10 @@ static struct {
 // poshold_init - initialise PosHold controller
 static bool poshold_init(bool ignore_checks)
 {
+    if (!ignore_checks && failsafe.gps_glitch) {
+        return false;
+    }
+
     // fail to initialise PosHold mode if no GPS lock
     if (!position_ok() && !ignore_checks) {
         return false;

ArduCopter/control_stop.pde

@@ -7,6 +7,10 @@
 // stop_init - initialise stop controller
 static bool stop_init(bool ignore_checks)
 {
+    if (!ignore_checks && failsafe.gps_glitch) {
+        return false;
+    }
+
     if (position_ok() || optflow_position_ok() || ignore_checks) {
 
         // set desired acceleration to zero

ArduCopter/gps_glitch.pde

@@ -0,0 +1,54 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+bool gps_glitch_switch_mode_on_resolve = false;
+
+static void gps_glitch_update() {
+    bool glitch = ahrs.get_NavEKF().getGpsGlitchStatus();
+
+    if (glitch && !failsafe.gps_glitch) {
+        gps_glitch_on_event();
+    } else if (!glitch && failsafe.gps_glitch) {
+        gps_glitch_off_event();
+    }
+
+    if (failsafe.gps_glitch && control_mode != ALT_HOLD) {
+        // if the mode has changed during gps glitch, don't return to LOITER on resolve
+        gps_glitch_switch_mode_on_resolve = false;
+    }
+}
+
+static bool gps_glitch_action_mode(uint8_t mode) {
+    switch(control_mode) {
+        case LAND:
+            return landing_with_GPS();
+        case RTL:
+            return rtl_state == Land;
+        case GUIDED: // GUIDED for solo because shots modes use GUIDED
+        case LOITER:
+        case DRIFT:
+        case STOP:
+        case POSHOLD:
+            return true;
+        default:
+            return false;
+    }
+    return false;
+}
+
+static void gps_glitch_on_event() {
+    failsafe.gps_glitch = true;
+
+    if (gps_glitch_action_mode(control_mode) && !failsafe.radio) {
+        gps_glitch_switch_mode_on_resolve = true;
+
+        set_mode(ALT_HOLD);
+    }
+}
+
+static void gps_glitch_off_event() {
+    failsafe.gps_glitch = false;
+
+    if (gps_glitch_switch_mode_on_resolve) {
+        set_mode(LOITER);
+    }
+}