added GPS check before mode change. New mode_requires_gps() function.

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

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