From f1751112438c67eb9c2dde5d27469750f751fb78 Mon Sep 17 00:00:00 2001
From: Dr Gareth Owen <drgowen@gmail.com>
Date: Wed, 29 May 2013 21:12:24 +0100
Subject: [PATCH] added GPS check before mode change. New mode_requires_gps()
 function.

---
 ArduCopter/events.pde | 26 ++------------------------
 ArduCopter/system.pde | 23 ++++++++++++++++++++---
 2 files changed, 22 insertions(+), 27 deletions(-)

diff --git a/ArduCopter/events.pde b/ArduCopter/events.pde
index 8802dd673b..daaf8a524d 100644
--- a/ArduCopter/events.pde
+++ b/ArduCopter/events.pde
@@ -138,30 +138,8 @@ static void failsafe_gps_check()
     Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GPS, ERROR_CODE_FAILSAFE_OCCURRED);
 
     // take action based on flight mode
-    switch(control_mode) {
-        // for modes that do not require gps, do nothing
-        case STABILIZE:
-        case ACRO:
-        case ALT_HOLD:
-        case OF_LOITER:
-            // do nothing
-            break;
-
-        // modes requiring GPS force a land
-        case AUTO:
-        case GUIDED:
-        case LOITER:
-        case RTL:
-        case CIRCLE:
-        case POSITION:
-            // We have no GPS or are very close to home so we will land
-            set_mode(LAND);
-            break;
-
-        case LAND:
-            // if we're already landing do nothing
-            break;
-    }
+    if(mode_requires_GPS(control_mode))
+        set_mode(LAND);
 }
 
 // failsafe_gps_off_event - actions to take when GPS contact is restored
diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde
index a3ffef4e23..fb385f22b1 100644
--- a/ArduCopter/system.pde
+++ b/ArduCopter/system.pde
@@ -312,14 +312,31 @@ static void startup_ground(void)
     reset_I_all();
 }
 
+// returns true or false whether mode requires GPS
+static bool mode_requires_GPS(uint8_t mode) {
+    switch(mode) {
+        case AUTO:
+        case GUIDED:
+        case LOITER: 
+        case RTL:
+        case CIRCLE:
+        case POSITION:
+            return true;
+        default:
+            return false;
+    }   
+
+    return false;
+}
+
 // set_mode - change flight mode and perform any necessary initialisation
 static void set_mode(uint8_t mode)
 {
     // Switch to stabilize mode if requested mode requires a GPS lock
-    if(!ap.home_is_set) {
-        if (mode > ALT_HOLD && mode != TOY_A && mode != TOY_M && mode != OF_LOITER && mode != LAND) {
+    if(g_gps->status() != GPS::GPS_OK_FIX_3D && mode_requires_GPS(mode)) {
+            mode = STABILIZE;
+    } else if(mode == RTL && !ap.home_is_set) {
             mode = STABILIZE;
-        }
     }
 
     // Switch to stabilize if OF_LOITER requested but no optical flow sensor