From 4686eef266ed35cc62bcd375b17d5b08ff16f2b5 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 27 Apr 2014 15:08:58 +0900 Subject: [PATCH] Copter: GPS failsafe ensures LAND is pilot-controlled This resolves issue #880 in which if a GPS failsafe occurred while the vehicle was already in LAND mode, the LAND would continue to be GPS controlled LAND instead of pilot-controlled LAND --- ArduCopter/control_land.pde | 8 ++++++++ ArduCopter/events.pde | 5 +++++ 2 files changed, 13 insertions(+) diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index cb999d40d6..13529add24 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -172,3 +172,11 @@ static bool update_land_detector() // return current state of landing return ap.land_complete; } + +// land_do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch +// called during GPS failsafe to ensure that if we were already in LAND mode that we do not use the GPS +// has no effect if we are not already in LAND mode +static bool land_do_not_use_GPS() +{ + land_with_gps = false; +} diff --git a/ArduCopter/events.pde b/ArduCopter/events.pde index e15baad2a5..904450d699 100644 --- a/ArduCopter/events.pde +++ b/ArduCopter/events.pde @@ -215,6 +215,11 @@ static void failsafe_gps_check() set_mode(LAND); } } + + // if flight mode is LAND ensure it's not the GPS controlled LAND + if (control_mode == LAND) { + land_do_not_use_GPS(); + } } // failsafe_gps_off_event - actions to take when GPS contact is restored