From 45a002d017bd3f9de667d9f278f79b24d124ca94 Mon Sep 17 00:00:00 2001 From: Nick Speal Date: Wed, 23 Aug 2017 12:13:51 -0700 Subject: [PATCH 1/2] After a GPS_GLITCH event while in a flight mode that does not require RC, switch mode into LAND --- ArduCopter/gps_glitch.pde | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/ArduCopter/gps_glitch.pde b/ArduCopter/gps_glitch.pde index bd372cf7c0..4ce86361ec 100644 --- a/ArduCopter/gps_glitch.pde +++ b/ArduCopter/gps_glitch.pde @@ -14,9 +14,15 @@ static void gps_glitch_on_event() { failsafe.gps_glitch = true; Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GPS, ERROR_CODE_FAILSAFE_OCCURRED); - if (motors.armed() && mode_requires_RC(control_mode) && mode_requires_GPS(control_mode) && !failsafe.radio && !failsafe.ekf) { - if(set_mode(ALT_HOLD)) { - gps_glitch_switch_mode_on_resolve = true; + if (motors.armed() && mode_requires_GPS(control_mode) && !failsafe.radio && !failsafe.ekf) { + if (mode_requires_RC(control_mode)) { + if(set_mode(ALT_HOLD)) { + gps_glitch_switch_mode_on_resolve = true; + } + } else { + if(set_mode(LAND)) { + gps_glitch_switch_mode_on_resolve = true; + } } } } From 16a29860ecad0e40360462b9311a7cd7531de9a9 Mon Sep 17 00:00:00 2001 From: Nick Speal Date: Fri, 1 Sep 2017 11:33:24 -0700 Subject: [PATCH 2/2] Do not use GPS if failsafe.gps_glitch is true when entering land mode --- ArduCopter/control_land.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index 79fe208c79..a629903064 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -16,7 +16,7 @@ static bool land_ramp; static bool land_init(bool ignore_checks) { // check if we have GPS and decide which LAND we're going to do - land_with_gps = position_ok(); + land_with_gps = position_ok() && !failsafe.gps_glitch; if (land_with_gps) { // set target to stopping point Vector3f stopping_point;