diff --git a/Blimp/Blimp.cpp b/Blimp/Blimp.cpp index 3f1ff9041b..e8406b28ac 100644 --- a/Blimp/Blimp.cpp +++ b/Blimp/Blimp.cpp @@ -46,6 +46,7 @@ const AP_Scheduler::Task Blimp::scheduler_tasks[] = { SCHED_TASK(one_hz_loop, 1, 100), SCHED_TASK(ekf_check, 10, 75), SCHED_TASK(check_vibration, 10, 50), + SCHED_TASK(gpsglitch_check, 10, 50), SCHED_TASK_CLASS(GCS, (GCS*)&blimp._gcs, update_receive, 400, 180), SCHED_TASK_CLASS(GCS, (GCS*)&blimp._gcs, update_send, 400, 550), #if LOGGING_ENABLED == ENABLED diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h index 53034366e5..04d39599c3 100644 --- a/Blimp/Blimp.h +++ b/Blimp/Blimp.h @@ -347,6 +347,7 @@ private: void failsafe_gcs_check(); bool should_disarm_on_failsafe(); void do_failsafe_action(Failsafe_Action action, ModeReason reason); + void gpsglitch_check(); // failsafe.cpp void failsafe_enable(); diff --git a/Blimp/events.cpp b/Blimp/events.cpp index 579fbb706e..7c3c5577a6 100644 --- a/Blimp/events.cpp +++ b/Blimp/events.cpp @@ -155,3 +155,23 @@ void Blimp::do_failsafe_action(Failsafe_Action action, ModeReason reason) break; } } + +// check for gps glitch failsafe +void Blimp::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) { + AP::logger().Write_Error(LogErrorSubsystem::GPS, LogErrorCode::GPS_GLITCH); + gcs().send_text(MAV_SEVERITY_CRITICAL,"GPS Glitch"); + } else { + AP::logger().Write_Error(LogErrorSubsystem::GPS, LogErrorCode::ERROR_RESOLVED); + gcs().send_text(MAV_SEVERITY_CRITICAL,"GPS Glitch cleared"); + } + } +} \ No newline at end of file