From bc1e06757bcd6512a4d73ddbbab0ef8b04c285f1 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 16 Nov 2013 13:55:59 +0900 Subject: [PATCH] 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 | 6 +++--- ArduCopter/config.h | 9 --------- ArduCopter/defines.h | 12 ++++++++++++ ArduCopter/events.pde | 18 ++++++++---------- 4 files changed, 23 insertions(+), 22 deletions(-) diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 247a71546d..407ad2279d 100644 --- a/ArduCopter/Parameters.pde +++ b/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 diff --git a/ArduCopter/config.h b/ArduCopter/config.h index d0dacc19e8..6ebf6a51b6 100644 --- a/ArduCopter/config.h +++ b/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 diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 548d719058..dd458a3b95 100644 --- a/ArduCopter/defines.h +++ b/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 diff --git a/ArduCopter/events.pde b/ArduCopter/events.pde index 73cab9a0c0..cc2afb393f 100644 --- a/ArduCopter/events.pde +++ b/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) { - set_mode(LAND); + // 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