From 63c032b8cc2b21e03e601ba7e19e04ae5f0695fc Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 25 Jul 2017 20:41:18 +0900 Subject: [PATCH] Copter: add gps glitch notification and pre-arm check --- ArduCopter/AP_Arming.cpp | 11 +++++++++++ ArduCopter/ArduCopter.cpp | 1 + ArduCopter/Copter.h | 3 ++- ArduCopter/defines.h | 4 +++- ArduCopter/events.cpp | 20 ++++++++++++++++++++ 5 files changed, 37 insertions(+), 2 deletions(-) diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index d6841ad4a9..f48bcadd74 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -435,6 +435,17 @@ bool AP_Arming_Copter::pre_arm_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 index dde68ff3fc..ca6d7ff8b8 100644 --- 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 index 979e3a6f92..8e601b1ea8 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -267,7 +267,7 @@ private: 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 @@ -955,6 +955,7 @@ private: 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 index a1fc3013c4..075c38e9c2 100644 --- 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 // Arming Check Enable/Disable bits #define ARMING_CHECK_NONE 0x00 diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index 36b78cd748..4f6b854e9f 100644 --- 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)