Copter: add gps glitch notification and pre-arm check

ArduPilot · Jul 27, 2017 · f97e439 · f97e439
1 parent 1ddf388
commit f97e439
Show file tree

Hide file tree

Showing 5 changed files with 37 additions and 2 deletions.
diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp
@@ -416,6 +416,17 @@ bool AP_Arming_Copter::gps_checks(bool display_failure)
         return false;
     }
 
+    // check for GPS glitch (as reported by EKF)
+    nav_filter_status filt_status;
+    if (_ahrs_navekf.get_filter_status(filt_status)) {
+        if (filt_status.flags.gps_glitching) {
+            if (display_failure) {
+                gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: GPS glitching");
+            }
+            return false;
+        }
+    }
+
     // check EKF compass variance is below failsafe threshold
     float vel_variance, pos_variance, hgt_variance, tas_variance;
     Vector3f mag_variance;

diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp
@@ -113,6 +113,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
     SCHED_TASK(update_notify,         50,     90),
     SCHED_TASK(one_hz_loop,            1,    100),
     SCHED_TASK(ekf_check,             10,     75),
+    SCHED_TASK(gpsglitch_check,       10,     50),
     SCHED_TASK(landinggear_update,    10,     75),
     SCHED_TASK(lost_vehicle_check,    10,     50),
     SCHED_TASK(gcs_check_input,      400,    180),

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
@@ -274,7 +274,7 @@ class Copter : public AP_HAL::HAL::Callbacks {
             uint8_t land_complete_maybe     : 1; // 14      // true if we may have landed (less strict version of land_complete)
             uint8_t throttle_zero           : 1; // 15      // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock
             uint8_t system_time_set         : 1; // 16      // true if the system time has been set from the GPS
-            uint8_t gps_base_pos_set        : 1; // 17      // true when the gps base position has been set (used for RTK gps only)
+            uint8_t gps_glitching           : 1; // 17      // true if the gps is glitching
             enum HomeState home_state       : 2; // 18,19   // home status (unset, set, locked)
             uint8_t using_interlock         : 1; // 20      // aux switch motor interlock function is in use
             uint8_t motor_emergency_stop    : 1; // 21      // motor estop switch, shuts off motors when enabled
@@ -974,6 +974,7 @@ class Copter : public AP_HAL::HAL::Callbacks {
     void failsafe_terrain_check();
     void failsafe_terrain_set_status(bool data_ok);
     void failsafe_terrain_on_event();
+    void gpsglitch_check();
     void set_mode_RTL_or_land_with_pause(mode_reason_t reason);
     void update_events();
     void failsafe_enable();

diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
@@ -408,7 +408,7 @@ enum DevOptions {
 #define ERROR_SUBSYSTEM_FAILSAFE_GCS        8
 #define ERROR_SUBSYSTEM_FAILSAFE_FENCE      9
 #define ERROR_SUBSYSTEM_FLIGHT_MODE         10
-#define ERROR_SUBSYSTEM_GPS                 11  // not used
+#define ERROR_SUBSYSTEM_GPS                 11
 #define ERROR_SUBSYSTEM_CRASH_CHECK         12
 #define ERROR_SUBSYSTEM_FLIP                13
 #define ERROR_SUBSYSTEM_AUTOTUNE            14
@@ -456,6 +456,8 @@ enum DevOptions {
 #define ERROR_CODE_EKFCHECK_VARIANCE_CLEARED   0
 // Baro specific error codes
 #define ERROR_CODE_BARO_GLITCH              2
+// GPS specific error coces
+#define ERROR_CODE_GPS_GLITCH               2
 
 // Radio failsafe definitions (FS_THR parameter)
 #define FS_THR_DISABLED                    0

diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp
@@ -184,6 +184,26 @@ void Copter::failsafe_terrain_on_event()
     }
 }
 
+// check for gps glitch failsafe
+void Copter::gpsglitch_check()
+{
+    // get filter status
+    nav_filter_status filt_status = inertial_nav.get_filter_status();
+    bool gps_glitching = filt_status.flags.gps_glitching;
+
+    // log start or stop of gps glitch.  AP_Notify update is handled from within AP_AHRS
+    if (ap.gps_glitching != gps_glitching) {
+        ap.gps_glitching = gps_glitching;
+        if (gps_glitching) {
+            Log_Write_Error(ERROR_SUBSYSTEM_GPS, ERROR_CODE_GPS_GLITCH);
+            gcs().send_text(MAV_SEVERITY_CRITICAL,"GPS Glitch");
+        } else {
+            Log_Write_Error(ERROR_SUBSYSTEM_GPS, ERROR_CODE_ERROR_RESOLVED);
+            gcs().send_text(MAV_SEVERITY_CRITICAL,"GPS Glitch cleared");
+        }
+    }
+}
+
 // set_mode_RTL_or_land_with_pause - sets mode to RTL if possible or LAND with 4 second delay before descent starts
 //  this is always called from a failsafe so we trigger notification to pilot
 void Copter::set_mode_RTL_or_land_with_pause(mode_reason_t reason)