Copter: allow GPS failsafe to trigger AltHold

FS_GPS_ENABLE parameter accepts two new options, 2=AltHold,
3=LandEvenFromStabilize.
If set to 3 the GPS failsafe will trigger and LAND even from manual
flight modes like Stabilize and ACRO.  This is useful for users who want
to ensure their copters can never stray outside the circular fence (the
fence only triggers when it knows it is outside the bounds, and it can't
know this if it has no GPS)

ArduCopter/Parameters.pde

@@ -120,10 +120,10 @@ const AP_Param::Info var_info[] PROGMEM = {
 
     // @Param: FS_GPS_ENABLE
     // @DisplayName: GPS Failsafe Enable
-    // @Description: Controls whether failsafe will be invoked when gps signal is lost
-    // @Values: 0:Disabled,1:Enabled
+    // @Description: Controls what action will be taken if GPS signal is lost for at least 5 seconds
+    // @Values: 0:Disabled,1:Land,2:AltHold,3:Land even from Stabilize
     // @User: Standard
-    GSCALAR(failsafe_gps_enabled, "FS_GPS_ENABLE", FS_GPS),
+    GSCALAR(failsafe_gps_enabled, "FS_GPS_ENABLE", FS_GPS_LAND),
 
     // @Param: FS_GCS_ENABLE
     // @DisplayName: Ground Station Failsafe Enable

ArduCopter/config.h

@@ -362,9 +362,6 @@
 #endif
 
 // GPS failsafe
-#ifndef FS_GPS
- # define FS_GPS                        ENABLED
-#endif
 #ifndef FAILSAFE_GPS_TIMEOUT_MS
  # define FAILSAFE_GPS_TIMEOUT_MS       5000    // gps failsafe triggers after 5 seconds with no GPS
 #endif
@@ -488,12 +485,6 @@
 //////////////////////////////////////////////////////////////////////////////
 // Throttle Failsafe
 //
-// possible values for FS_THR parameter
-#define FS_THR_DISABLED                    0
-#define FS_THR_ENABLED_ALWAYS_RTL          1
-#define FS_THR_ENABLED_CONTINUE_MISSION    2
-#define FS_THR_ENABLED_ALWAYS_LAND         3
-
 #ifndef FS_THR_VALUE_DEFAULT
  # define FS_THR_VALUE_DEFAULT             975
 #endif

ArduCopter/defines.h

@@ -479,4 +479,16 @@ enum ap_message {
 #define ARMING_CHECK_RC                     0x40
 #define ARMING_CHECK_VOLTAGE                0x80
 
+// Radio failsafe definitions (FS_THR parameter)
+#define FS_THR_DISABLED                    0
+#define FS_THR_ENABLED_ALWAYS_RTL          1
+#define FS_THR_ENABLED_CONTINUE_MISSION    2
+#define FS_THR_ENABLED_ALWAYS_LAND         3
+
+// GPS Failsafe definitions (FS_GPS_ENABLE parameter)
+#define FS_GPS_DISABLED                     0       // GPS failsafe disabled
+#define FS_GPS_LAND                         1       // switch to LAND mode on GPS Failsafe
+#define FS_GPS_ALTHOLD                      2       // switch to ALTHOLD mode on GPS failsafe
+#define FS_GPS_LAND_EVEN_STABILIZE          3       // switch to LAND mode on GPS failsafe even if in a manual flight mode like Stabilize
+
 #endif // _DEFINES_H

ArduCopter/events.pde

@@ -138,7 +138,7 @@ static void failsafe_gps_check()
     uint32_t last_gps_update_ms;
 
     // return immediately if gps failsafe is disabled or we have never had GPS lock
-    if (!g.failsafe_gps_enabled || !ap.home_is_set) {
+    if (g.failsafe_gps_enabled == FS_GPS_DISABLED || !ap.home_is_set) {
         // if we have just disabled the gps failsafe, ensure the gps failsafe event is cleared
         if (failsafe.gps) {
             failsafe_gps_off_event();
@@ -171,16 +171,14 @@ static void failsafe_gps_check()
     gcs_send_text_P(SEVERITY_LOW,PSTR("Lost GPS!"));
     Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GPS, ERROR_CODE_FAILSAFE_OCCURRED);
 
-    // take action based on flight mode
-    if(mode_requires_GPS(control_mode))
-        set_mode(LAND);
-
-    // land if circular fence is enabled
-#if AC_FENCE == ENABLED
-    if((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) {
+    // take action based on flight mode and FS_GPS_ENABLED parameter
+    if (mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) {
+        if (g.failsafe_gps_enabled == FS_GPS_ALTHOLD && !failsafe.radio) {
+            set_mode(ALT_HOLD);
+        }else{
             set_mode(LAND);
         }
-#endif
+    }
 }
 
 // failsafe_gps_off_event - actions to take when GPS contact is restored