Copter: remove GPS glitch and failsafe

The EKF failsafe now captures all failures that could lead to a bad
position including GPS glitches and a bad compass meaning we do not need
this protection in the main flight code.

ArduCopter/AP_State.pde

@@ -86,16 +86,6 @@ void set_failsafe_battery(bool b)
     AP_Notify::flags.failsafe_battery = b;
 }
 
-
-// ---------------------------------------------
-static void set_failsafe_gps(bool b)
-{
-    failsafe.gps = b;
-
-    // update AP_Notify
-    AP_Notify::flags.failsafe_gps = b;
-}
-
 // ---------------------------------------------
 static void set_failsafe_gcs(bool b)
 {

ArduCopter/ArduCopter.pde

@@ -103,7 +103,6 @@
 #include <GCS_MAVLink.h>        // MAVLink GCS definitions
 #include <AP_SerialManager.h>   // Serial manager library
 #include <AP_GPS.h>             // ArduPilot GPS library
-#include <AP_GPS_Glitch.h>      // GPS glitch protection library
 #include <DataFlash.h>          // ArduPilot Mega Flash Memory Library
 #include <AP_ADC.h>             // ArduPilot Mega Analog to Digital Converter Library
 #include <AP_ADC_AnalogSource.h>
@@ -257,8 +256,6 @@ static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor:
 
 static AP_GPS  gps;
 
-static GPS_Glitch gps_glitch(gps);
-
 // flight modes convenience array
 static AP_Int8 *flight_modes = &g.flight_mode1;
 
@@ -408,7 +405,6 @@ static struct {
     uint8_t rc_override_active  : 1; // 0   // true if rc control are overwritten by ground station
     uint8_t radio               : 1; // 1   // A status flag for the radio failsafe
     uint8_t battery             : 1; // 2   // A status flag for the battery failsafe
-    uint8_t gps                 : 1; // 3   // A status flag for the gps 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
 
@@ -602,7 +598,7 @@ static float G_Dt = 0.02;
 ////////////////////////////////////////////////////////////////////////////////
 // Inertial Navigation
 ////////////////////////////////////////////////////////////////////////////////
-static AP_InertialNav_NavEKF inertial_nav(ahrs, barometer, gps_glitch);
+static AP_InertialNav_NavEKF inertial_nav(ahrs);
 
 ////////////////////////////////////////////////////////////////////////////////
 // Attitude, Position and Waypoint navigation objects
@@ -1175,12 +1171,11 @@ static void one_hz_loop()
 static void update_GPS(void)
 {
     static uint32_t last_gps_reading[GPS_MAX_INSTANCES];   // time of last gps message
-    bool report_gps_glitch;
     bool gps_updated = false;
 
     gps.update();
 
-    // logging and glitch protection run after every gps message
+    // log after every gps message
     for (uint8_t i=0; i<gps.num_sensors(); i++) {
         if (gps.last_message_time_ms(i) != last_gps_reading[i]) {
             last_gps_reading[i] = gps.last_message_time_ms(i);
@@ -1195,20 +1190,6 @@ static void update_GPS(void)
     }
 
     if (gps_updated) {
-        // run glitch protection and update AP_Notify if home has been initialised
-        if (home_is_set()) {
-            gps_glitch.check_position();
-            report_gps_glitch = (gps_glitch.glitching() && !ap.usb_connected && hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
-            if (AP_Notify::flags.gps_glitching != report_gps_glitch) {
-                if (gps_glitch.glitching()) {
-                    Log_Write_Error(ERROR_SUBSYSTEM_GPS, ERROR_CODE_GPS_GLITCH);
-                }else{
-                    Log_Write_Error(ERROR_SUBSYSTEM_GPS, ERROR_CODE_ERROR_RESOLVED);
-                }
-                AP_Notify::flags.gps_glitching = report_gps_glitch;
-            }
-        }
-
         // set system time if necessary
         set_system_time_from_GPS();
 
@@ -1228,9 +1209,6 @@ static void update_GPS(void)
 #endif
         }
     }
-
-    // check for loss of gps
-    failsafe_gps_check();
 }
 
 static void

ArduCopter/GCS_Mavlink.pde

@@ -43,7 +43,7 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
     uint32_t custom_mode = control_mode;
 
     // set system as critical if any failsafe have triggered
-    if (failsafe.radio || failsafe.battery || failsafe.gps || failsafe.gcs || failsafe.ekf)  {
+    if (failsafe.radio || failsafe.battery || failsafe.gcs || failsafe.ekf)  {
         system_status = MAV_STATE_CRITICAL;
     }
 
@@ -197,7 +197,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan)
     if (g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) {
         control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
     }
-    if (gps.status() > AP_GPS::NO_GPS && (!gps_glitch.glitching()||ap.usb_connected)) {
+    if (gps.status() > AP_GPS::NO_GPS) {
         control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
     }
 #if OPTFLOW == ENABLED

ArduCopter/Parameters.h

@@ -135,7 +135,7 @@ public:
         k_param_geofence_limit,         // deprecated - remove
         k_param_altitude_limit,         // deprecated - remove
         k_param_fence,
-        k_param_gps_glitch,
+        k_param_gps_glitch,             // deprecated
         k_param_baro_glitch,            // 71 - deprecated
 
         //
@@ -258,7 +258,7 @@ public:
         k_param_rc_speed = 192,
         k_param_failsafe_battery_enabled,
         k_param_throttle_mid,
-        k_param_failsafe_gps_enabled,
+        k_param_failsafe_gps_enabled,   // remove
         k_param_rc_9,
         k_param_rc_12,
         k_param_failsafe_gcs,           // 198
@@ -342,7 +342,6 @@ public:
     AP_Float        fs_batt_voltage;            // battery voltage below which failsafe will be triggered
     AP_Float        fs_batt_mah;                // battery capacity (in mah) below which failsafe will be triggered
 
-    AP_Int8         failsafe_gps_enabled;       // gps failsafe enabled
     AP_Int8         failsafe_gcs;               // ground station failsafe behavior
     AP_Int16        gps_hdop_good;              // GPS Hdop value at or below this value represent a good position
 

ArduCopter/Parameters.pde

@@ -115,13 +115,6 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Standard
     GSCALAR(fs_batt_mah,            "FS_BATT_MAH", FS_BATT_MAH_DEFAULT),
 
-    // @Param: FS_GPS_ENABLE
-    // @DisplayName: GPS Failsafe Enable
-    // @Description: Controls what action will be taken if GPS signal is lost for at least 5 seconds
-    // @Values: 0:Disabled,1:Land,2:AltHold,3:Land even from Stabilize
-    // @User: Standard
-    GSCALAR(failsafe_gps_enabled, "FS_GPS_ENABLE", FS_GPS_LAND),
-
     // @Param: FS_GCS_ENABLE
     // @DisplayName: Ground Station Failsafe Enable
     // @Description: Controls whether failsafe will be invoked (and what action to take) when connection with Ground station is lost for at least 5 seconds. NB. The GCS Failsafe is only active when RC_OVERRIDE is being used to control the vehicle.
@@ -899,10 +892,6 @@ const AP_Param::Info var_info[] PROGMEM = {
     GOBJECT(rally,      "RALLY_",   AP_Rally),
 #endif
 
-    // @Group: GPSGLITCH_
-    // @Path: ../libraries/AP_GPS/AP_GPS_Glitch.cpp
-    GOBJECT(gps_glitch,      "GPSGLITCH_",   GPS_Glitch),
-
 #if FRAME_CONFIG ==     HELI_FRAME
     // @Group: H_
     // @Path: ../libraries/AP_Motors/AP_MotorsHeli.cpp

ArduCopter/compassmot.pde

@@ -101,7 +101,6 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
     // disable throttle and battery failsafe
     g.failsafe_throttle = FS_THR_DISABLED;
     g.failsafe_battery_enabled = FS_BATT_DISABLED;
-    g.failsafe_gps_enabled = FS_GPS_DISABLED;
 
     // disable motor compensation
     compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED);
@@ -259,7 +258,6 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
     // re-enable failsafes
     g.failsafe_throttle.load();
     g.failsafe_battery_enabled.load();
-    g.failsafe_gps_enabled.load();
 
     // flag we have completed
     ap.compass_mot = false;

ArduCopter/config.h

@@ -274,10 +274,7 @@
  # define BOARD_VOLTAGE_MAX             5.8f        // max board voltage in volts for pre-arm checks
 #endif
 
-// GPS failsafe
-#ifndef FAILSAFE_GPS_TIMEOUT_MS
- # define FAILSAFE_GPS_TIMEOUT_MS       5000    // gps failsafe triggers after 5 seconds with no GPS
-#endif
+// prearm GPS hdop check
 #ifndef GPS_HDOP_GOOD_DEFAULT
  # define GPS_HDOP_GOOD_DEFAULT         230     // minimum hdop that represents a good position.  used during pre-arm checks if fence is enabled
 #endif

ArduCopter/defines.h

@@ -320,11 +320,11 @@ enum FlipState {
 #define ERROR_SUBSYSTEM_OPTFLOW             4
 #define ERROR_SUBSYSTEM_FAILSAFE_RADIO      5
 #define ERROR_SUBSYSTEM_FAILSAFE_BATT       6
-#define ERROR_SUBSYSTEM_FAILSAFE_GPS        7
+#define ERROR_SUBSYSTEM_FAILSAFE_GPS        7   // not used
 #define ERROR_SUBSYSTEM_FAILSAFE_GCS        8
 #define ERROR_SUBSYSTEM_FAILSAFE_FENCE      9
 #define ERROR_SUBSYSTEM_FLIGHT_MODE         10
-#define ERROR_SUBSYSTEM_GPS                 11
+#define ERROR_SUBSYSTEM_GPS                 11  // not used
 #define ERROR_SUBSYSTEM_CRASH_CHECK         12
 #define ERROR_SUBSYSTEM_FLIP                13
 #define ERROR_SUBSYSTEM_AUTOTUNE            14
@@ -343,8 +343,6 @@ enum FlipState {
 #define ERROR_CODE_FAILSAFE_OCCURRED        1
 // subsystem specific error codes -- compass
 #define ERROR_CODE_COMPASS_FAILED_TO_READ   2
-// subsystem specific error codes -- gps
-#define ERROR_CODE_GPS_GLITCH               2
 // subsystem specific error codes -- main
 #define ERROR_CODE_MAIN_INS_DELAY           1
 // subsystem specific error codes -- crash checker

ArduCopter/events.pde

@@ -162,69 +162,6 @@ static void failsafe_battery_event(void)
 
 }
 
-// failsafe_gps_check - check for gps failsafe
-static void failsafe_gps_check()
-{
-    uint32_t last_gps_update_ms;
-
-    // return immediately if gps failsafe is disabled or we have never had GPS lock
-    if (g.failsafe_gps_enabled == FS_GPS_DISABLED || !home_is_set()) {
-        // if we have just disabled the gps failsafe, ensure the gps failsafe event is cleared
-        if (failsafe.gps) {
-            failsafe_gps_off_event();
-            set_failsafe_gps(false);
-        }
-        return;
-    }
-
-    // calc time since last gps update
-    last_gps_update_ms = millis() - gps_glitch.last_good_update();
-
-    // check if all is well
-    if( last_gps_update_ms < FAILSAFE_GPS_TIMEOUT_MS) {
-        // check for recovery from gps failsafe
-        if( failsafe.gps ) {
-            failsafe_gps_off_event();
-            set_failsafe_gps(false);
-        }
-        return;
-    }
-
-    // do nothing if gps failsafe already triggered or motors disarmed
-    if( failsafe.gps || !motors.armed()) {
-        return;
-    }
-
-    // GPS failsafe event has occured
-    // update state, warn the ground station and log to dataflash
-    set_failsafe_gps(true);
-    gcs_send_text_P(SEVERITY_HIGH,PSTR("Lost GPS!"));
-    Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GPS, ERROR_CODE_FAILSAFE_OCCURRED);
-
-    // take action based on flight mode and FS_GPS_ENABLED parameter
-    if (mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) {
-        if (g.failsafe_gps_enabled == FS_GPS_ALTHOLD && !failsafe.radio) {
-            set_mode(ALT_HOLD);
-            // alert pilot to mode change
-            AP_Notify::events.failsafe_mode_change = 1;
-        }else{
-            set_mode_land_with_pause();
-        }
-    }
-
-    // if flight mode is LAND ensure it's not the GPS controlled LAND
-    if (control_mode == LAND) {
-        land_do_not_use_GPS();
-    }
-}
-
-// failsafe_gps_off_event - actions to take when GPS contact is restored
-static void failsafe_gps_off_event(void)
-{
-    // log recovery of GPS in logs?
-    Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GPS, ERROR_CODE_FAILSAFE_RESOLVED);
-}
-
 // failsafe_gcs_check - check for ground station failsafe
 static void failsafe_gcs_check()
 {

ArduCopter/motor_test.pde

@@ -118,7 +118,6 @@ static uint8_t mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_se
             // disable throttle, battery and gps failsafe
             g.failsafe_throttle = FS_THR_DISABLED;
             g.failsafe_battery_enabled = FS_BATT_DISABLED;
-            g.failsafe_gps_enabled = FS_GPS_DISABLED;
             g.failsafe_gcs = FS_GCS_DISABLED;
 
             // turn on notify leds
@@ -160,7 +159,6 @@ static void motor_test_stop()
     // re-enable failsafes
     g.failsafe_throttle.load();
     g.failsafe_battery_enabled.load();
-    g.failsafe_gps_enabled.load();
     g.failsafe_gcs.load();
 
     // turn off notify leds

ArduCopter/motors.pde

@@ -499,11 +499,6 @@ static bool pre_arm_gps_checks(bool display_failure)
     // check if flight mode requires GPS
     bool gps_required = mode_requires_GPS(control_mode);
 
-    // if GPS failsafe will triggers even in stabilize mode we need GPS before arming
-    if (g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) {
-        gps_required = true;
-    }
-
 #if AC_FENCE == ENABLED
     // if circular fence is enabled we need GPS
     if ((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) {
@@ -517,15 +512,6 @@ static bool pre_arm_gps_checks(bool display_failure)
         return true;
     }
 
-    // check GPS is not glitching
-    if (gps_glitch.glitching()) {
-        if (display_failure) {
-            gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: GPS Glitch"));
-        }
-        AP_Notify::flags.pre_arm_gps_check = false;
-        return false;
-    }
-
     // ensure GPS is ok
     if (!position_ok()) {
         if (display_failure) {