Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Blimp: Add GPS glitch check

This commit is contained in:
Michelle Rossouw 2021-07-01 14:31:45 +10:00 committed by Andrew Tridgell
parent 8017e528c8
commit 6ec221625b
3 changed files with 22 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Blimp/Blimp.cpp
View File

@ -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

1
Blimp/Blimp.h
View File

@ -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();

20
Blimp/events.cpp
View File

@ -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");
}
}
}