Copter: Refactor failsafes, add fs_options bitmask parameter

- Radio failsafe, battery failsafe, GCS failsafe refactoring
- Add new FS_OPTIONS parameter
- Enhance GCS Failsafe abilities

ArduCopter/Copter.h

@@ -616,6 +616,14 @@ private:
         Failsafe_Action_Terminate      = 5
     };
 
+    enum class FailsafeOption {
+        RC_CONTINUE_IF_AUTO             = (1<<0),   // 1
+        GCS_CONTINUE_IF_AUTO            = (1<<1),   // 2
+        RC_CONTINUE_IF_GUIDED           = (1<<2),   // 4
+        CONTINUE_IF_LANDING             = (1<<3),   // 8
+        GCS_CONTINUE_IF_PILOT_CONTROL   = (1<<4),   // 16
+    };
+
     static constexpr int8_t _failsafe_priorities[] = {
                                                       Failsafe_Action_Terminate,
                                                       Failsafe_Action_Land,
@@ -710,10 +718,12 @@ private:
     void esc_calibration_setup();
 
     // events.cpp
+    bool failsafe_option(FailsafeOption opt) const;
     void failsafe_radio_on_event();
     void failsafe_radio_off_event();
     void handle_battery_failsafe(const char* type_str, const int8_t action);
     void failsafe_gcs_check();
+    void failsafe_gcs_on_event(void);
     void failsafe_gcs_off_event(void);
     void failsafe_terrain_check();
     void failsafe_terrain_set_status(bool data_ok);
@@ -723,6 +733,7 @@ private:
     void set_mode_SmartRTL_or_RTL(ModeReason reason);
     void set_mode_SmartRTL_or_land_with_pause(ModeReason reason);
     bool should_disarm_on_failsafe();
+    void do_failsafe_action(Failsafe_Action action, ModeReason reason);
 
     // failsafe.cpp
     void failsafe_enable();
@@ -813,6 +824,7 @@ private:
     void convert_pid_parameters(void);
     void convert_lgr_parameters(void);
     void convert_tradheli_parameters(void);
+    void convert_fs_options_params(void);
 
     // precision_landing.cpp
     void init_precland();

ArduCopter/Parameters.cpp

@@ -173,7 +173,7 @@ const AP_Param::Info Copter::var_info[] = {
     // @Param: FS_GCS_ENABLE
     // @DisplayName: Ground Station Failsafe Enable
     // @Description: Controls whether failsafe will be invoked (and what action to take) when connection with Ground station is lost for at least 5 seconds. NB. The GCS Failsafe is only active when RC_OVERRIDE is being used to control the vehicle.
-    // @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode,3:Enabled always SmartRTL or RTL,4:Enabled always SmartRTL or Land
+    // @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode (Deprecated in 4.0+),3:Enabled always SmartRTL or RTL,4:Enabled always SmartRTL or Land,5:Enabled always land (4.0+ Only)
     // @User: Standard
     GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_ENABLED_ALWAYS_RTL),
 
@@ -237,7 +237,7 @@ const AP_Param::Info Copter::var_info[] = {
     // @Param: FS_THR_ENABLE
     // @DisplayName: Throttle Failsafe Enable
     // @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel
-    // @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode,3:Enabled always Land,4:Enabled always SmartRTL or RTL,5:Enabled always SmartRTL or Land
+    // @Values:  0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode (Deprecated in 4.0+),3:Enabled always Land,4:Enabled always SmartRTL or RTL,5:Enabled always SmartRTL or Land
     // @User: Standard
     GSCALAR(failsafe_throttle,  "FS_THR_ENABLE",   FS_THR_ENABLED_ALWAYS_RTL),
 
@@ -939,7 +939,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Path: mode_systemid.cpp
     AP_SUBGROUPPTR(mode_systemid_ptr, "SID", 34, ParametersG2, ModeSystemId),
 #endif
-    
+
     // @Param: FS_VIBE_ENABLE
     // @DisplayName: Vibration Failsafe enable
     // @Description: This enables the vibration failsafe which will use modified altitude estimation and control during high vibrations
@@ -947,6 +947,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("FS_VIBE_ENABLE", 35, ParametersG2, fs_vibe_enabled, 1),
 
+    // @Param: FS_OPTIONS
+    // @DisplayName: Failsafe options bitmask
+    // @Description: Bitmask of additional options for battery, radio, & GCS failsafes. 0 (default) disables all options.
+    // @Values: 0:Disabled, 1:Continue if in Auto on RC failsafe only, 2:Continue if in Auto on GCS failsafe only, 3:Continue if in Auto on RC and/or GCS failsafe, 4:Continue if in Guided on RC failsafe only, 8:Continue if landing on any failsafe, 16:Continue if in pilot controlled modes on GCS failsafe, 19:Continue if in Auto on RC and/or GCS failsafe and continue if in pilot controlled modes on GCS failsafe
+    // @Bitmask: 0:Continue if in Auto on RC failsafe, 1:Continue if in Auto on GCS failsafe, 2:Continue if in Guided on RC failsafe, 3:Continue if landing on any failsafe, 4:Continue if in pilot controlled modes on GCS failsafe
+    // @User: Advanced
+    AP_GROUPINFO("FS_OPTIONS", 36, ParametersG2, fs_options, 0),
+
     AP_GROUPEND
 };
 
@@ -1103,6 +1111,9 @@ void Copter::load_parameters(void)
     // convert landing gear parameters
     convert_lgr_parameters();
 
+    // convert fs_options parameters
+    convert_fs_options_params();
+
     hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before));
 
     // setup AP_Param frame type flags
@@ -1528,3 +1539,33 @@ void Copter::convert_tradheli_parameters(void)
 
 }
 #endif
+
+void Copter::convert_fs_options_params(void)
+{
+    // If FS_OPTIONS has already been configured and we don't change it.
+    enum ap_var_type ptype;
+    AP_Int32 *fs_opt = (AP_Int32 *)AP_Param::find("FS_OPTIONS", &ptype);
+
+    if (fs_opt == nullptr || fs_opt->configured_in_storage() || ptype != AP_PARAM_INT32) {
+        return;
+    }
+
+    // Variable to capture the new FS_OPTIONS setting
+    int32_t fs_options_converted = 0;
+
+    // If FS_THR_ENABLED is 2 (continue mission), change to RTL and add continue mission to the new FS_OPTIONS parameter
+    if (g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) {
+        fs_options_converted |= int32_t(FailsafeOption::RC_CONTINUE_IF_AUTO);
+        AP_Param::set_and_save_by_name("FS_THR_ENABLE", FS_THR_ENABLED_ALWAYS_RTL);
+    }
+
+    // If FS_GCS_ENABLED is 2 (continue mission), change to RTL and add continue mission to the new FS_OPTIONS parameter
+    if (g.failsafe_gcs == FS_GCS_ENABLED_CONTINUE_MISSION) {
+        fs_options_converted |= int32_t(FailsafeOption::GCS_CONTINUE_IF_AUTO);
+        AP_Param::set_and_save_by_name("FS_GCS_ENABLE", FS_GCS_ENABLED_ALWAYS_RTL);
+    }
+
+    // Write the new value to FS_OPTIONS
+    // AP_Param::set_and_save_by_name("FS_OPTIONS", fs_options_converted);
+    fs_opt->set_and_save(fs_options_converted);
+}

ArduCopter/Parameters.h

@@ -612,6 +612,9 @@ public:
 
     // vibration failsafe enable/disable
     AP_Int8 fs_vibe_enabled;
+
+    // Failsafe options bitmask #36
+    AP_Int32 fs_options;
 };
 
 extern const AP_Param::Info        var_info[];

ArduCopter/defines.h

@@ -204,7 +204,7 @@ enum HarmonicNotchDynamicMode {
 // 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_CONTINUE_MISSION            2    // Deprecated in 4.0+, now use fs_options
 #define FS_THR_ENABLED_ALWAYS_LAND                 3
 #define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL      4
 #define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND     5
@@ -212,9 +212,10 @@ enum HarmonicNotchDynamicMode {
 // GCS failsafe definitions (FS_GCS_ENABLE parameter)
 #define FS_GCS_DISABLED                        0
 #define FS_GCS_ENABLED_ALWAYS_RTL              1
-#define FS_GCS_ENABLED_CONTINUE_MISSION        2
+#define FS_GCS_ENABLED_CONTINUE_MISSION        2    // Deprecated in 4.0+, now use fs_options
 #define FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL  3
 #define FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND 4
+#define FS_GCS_ENABLED_ALWAYS_LAND             5
 
 // EKF failsafe definitions (FS_EKF_ACTION parameter)
 #define FS_EKF_ACTION_LAND                  1       // switch to LAND mode on EKF failsafe

ArduCopter/events.cpp

@@ -4,104 +4,115 @@
  *       This event will be called when the failsafe changes
  *       boolean failsafe reflects the current state
  */
+
+bool Copter::failsafe_option(FailsafeOption opt) const
+{
+    return (g2.fs_options & (uint32_t)opt);
+}
+
 void Copter::failsafe_radio_on_event()
 {
-    // if motors are not armed there is nothing to do
-    if( !motors->armed() ) {
-        return;
-    }
-
-    if (should_disarm_on_failsafe()) {
-        arming.disarm();
-    } else {
-        if (control_mode == Mode::Number::AUTO && g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) {
-            // continue mission
-        } else if (flightmode->is_landing() &&
-                   battery.has_failsafed() &&
-                   battery.get_highest_failsafe_priority() <= FAILSAFE_LAND_PRIORITY) {
-            // continue landing or other high priority failsafes
-        } else {
-            if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) {
-                set_mode_RTL_or_land_with_pause(ModeReason::RADIO_FAILSAFE);
-            } else if (g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) {
-                set_mode_RTL_or_land_with_pause(ModeReason::RADIO_FAILSAFE);
-            } else if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL) {
-                set_mode_SmartRTL_or_RTL(ModeReason::RADIO_FAILSAFE);
-            } else if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND) {
-                set_mode_SmartRTL_or_land_with_pause(ModeReason::RADIO_FAILSAFE);
-            } else { // g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND
-                set_mode_land_with_pause(ModeReason::RADIO_FAILSAFE);
-            }
-        }
-    }
-
     AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_OCCURRED);
 
+    // set desired action based on FS_THR_ENABLE parameter
+    Failsafe_Action desired_action;
+    switch (g.failsafe_throttle) {
+        case FS_THR_DISABLED:
+            desired_action = Failsafe_Action_None;
+            break;
+        case FS_THR_ENABLED_ALWAYS_RTL:
+        case FS_THR_ENABLED_CONTINUE_MISSION:
+            desired_action = Failsafe_Action_RTL;
+            break;
+        case FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL:
+            desired_action = Failsafe_Action_SmartRTL;
+            break;
+        case FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND:
+            desired_action = Failsafe_Action_SmartRTL_Land;
+            break;
+        case FS_THR_ENABLED_ALWAYS_LAND:
+            desired_action = Failsafe_Action_Land;
+            break;
+        default:
+            desired_action = Failsafe_Action_Land;
+    }
+
+    // Conditions to deviate from FS_THR_ENABLE selection and send specific GCS warning
+    if (should_disarm_on_failsafe()) {
+        // should immediately disarm when we're on the ground
+        gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Disarming");
+        arming.disarm();
+        desired_action = Failsafe_Action_None;
+
+    } else if (flightmode->is_landing() && ((battery.has_failsafed() && battery.get_highest_failsafe_priority() <= FAILSAFE_LAND_PRIORITY))) {
+        // Allow landing to continue when battery failsafe requires it (not a user option)
+        gcs().send_text(MAV_SEVERITY_WARNING, "Radio + Battery Failsafe - Continuing Landing");
+        desired_action = Failsafe_Action_Land;
+
+    } else if (flightmode->is_landing() && failsafe_option(FailsafeOption::CONTINUE_IF_LANDING)) {
+        // Allow landing to continue when FS_OPTIONS is set to continue landing
+        gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Continuing Landing");
+        desired_action = Failsafe_Action_Land;
+
+    } else if (control_mode == Mode::Number::AUTO && failsafe_option(FailsafeOption::RC_CONTINUE_IF_AUTO)) {
+        // Allow mission to continue when FS_OPTIONS is set to continue mission
+        gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Continuing Auto Mode");       
+        desired_action = Failsafe_Action_None;
+
+    } else if ((flightmode->in_guided_mode()) &&
+      (failsafe_option(FailsafeOption::RC_CONTINUE_IF_GUIDED)) && (g.failsafe_gcs != FS_GCS_DISABLED)) {
+        // Allow guided mode to continue when FS_OPTIONS is set to continue in guided mode.  Only if the GCS failsafe is enabled.
+        gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Continuing Guided Mode");
+        desired_action = Failsafe_Action_None;
+
+    } else {
+        gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe");
+    }
+
+    // Call the failsafe action handler
+    do_failsafe_action(desired_action, ModeReason::RADIO_FAILSAFE);
 }
 
 // failsafe_off_event - respond to radio contact being regained
-// we must be in AUTO, LAND or RTL modes
-// or Stabilize or ACRO mode but with motors disarmed
 void Copter::failsafe_radio_off_event()
 {
     // no need to do anything except log the error as resolved
     // user can now override roll, pitch, yaw and throttle and even use flight mode switch to restore previous flight mode
     AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_RESOLVED);
+    gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe Cleared");
 }
 
 void Copter::handle_battery_failsafe(const char *type_str, const int8_t action)
 {
     AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_BATT, LogErrorCode::FAILSAFE_OCCURRED);
 
-    // failsafe check
+    Failsafe_Action desired_action = (Failsafe_Action)action;
+
+    // Conditions to deviate from BATT_FS_XXX_ACT parameter setting
     if (should_disarm_on_failsafe()) {
+        // should immediately disarm when we're on the ground
         arming.disarm();
+        desired_action = Failsafe_Action_None;
+        gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe - Disarming");
+
+    } else if (flightmode->is_landing() && failsafe_option(FailsafeOption::CONTINUE_IF_LANDING) && desired_action != Failsafe_Action_None) {
+        // Allow landing to continue when FS_OPTIONS is set to continue when landing
+        desired_action = Failsafe_Action_Land;
+        gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe - Continuing Landing");
     } else {
-        switch ((Failsafe_Action)action) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe");
-            case Failsafe_Action_None:
-                return;
-            case Failsafe_Action_Land:
-                set_mode_land_with_pause(ModeReason::BATTERY_FAILSAFE);
-                break;
-            case Failsafe_Action_RTL:
-                set_mode_RTL_or_land_with_pause(ModeReason::BATTERY_FAILSAFE);
-                break;
-            case Failsafe_Action_SmartRTL:
-                set_mode_SmartRTL_or_RTL(ModeReason::BATTERY_FAILSAFE);
-                break;
-            case Failsafe_Action_SmartRTL_Land:
-                set_mode_SmartRTL_or_land_with_pause(ModeReason::BATTERY_FAILSAFE);
-                break;
-            case Failsafe_Action_Terminate:
-#if ADVANCED_FAILSAFE == ENABLED
-                char battery_type_str[17];
-                snprintf(battery_type_str, 17, "%s battery", type_str);
-                g2.afs.gcs_terminate(true, battery_type_str);
-#else
-                arming.disarm();
-#endif
-        }
     }
+
+    // Battery FS options already use the Failsafe_Options enum. So use them directly.
+    do_failsafe_action(desired_action, ModeReason::BATTERY_FAILSAFE);
+
 }
 
 // failsafe_gcs_check - check for ground station failsafe
 void Copter::failsafe_gcs_check()
 {
-    if (failsafe.gcs) {
+    // Bypass GCS failsafe checks if disabled or GCS never connected
-        // we must run the failsafe checks if we are in failsafe -
+    if (g.failsafe_gcs == FS_GCS_DISABLED || failsafe.last_heartbeat_ms == 0) {
-        // otherwise we will never leave failsafe
-    } else if (g.failsafe_gcs == FS_GCS_DISABLED) {
-        // simply disabled
-        return;
-    } else if (failsafe.last_heartbeat_ms == 0) {
-        // GCS has never connected
-        return;
-    } else if (RC_Channels::has_active_overrides()) {
-        // GCS is currently telling us what to do!
-    } else if (control_mode == Mode::Number::GUIDED ||
-               control_mode == Mode::Number::GUIDED_NOGPS) {
-        // GCS is currently telling us what to do!
-    } else {
         return;
     }
 
@@ -109,47 +120,96 @@ void Copter::failsafe_gcs_check()
     // note: this only looks at the heartbeat from the device id set by g.sysid_my_gcs
     const uint32_t last_gcs_update_ms = millis() - failsafe.last_heartbeat_ms;
 
-    // check if all is well
+    // Determine which event to trigger
-    if (last_gcs_update_ms < FS_GCS_TIMEOUT_MS) {
+    if (last_gcs_update_ms < FS_GCS_TIMEOUT_MS && failsafe.gcs) {
-        // check for recovery from gcs failsafe
+        // Recovery from a GCS failsafe
-        if (failsafe.gcs) {
+        set_failsafe_gcs(false);
-            failsafe_gcs_off_event();
+        failsafe_gcs_off_event();
-            set_failsafe_gcs(false);
-        }
-        return;
-    }
 
-    // do nothing if gcs failsafe already triggered or motors disarmed
+    } else if (last_gcs_update_ms < FS_GCS_TIMEOUT_MS && !failsafe.gcs) {
-    if (failsafe.gcs || !motors->armed()) {
+        // No problem, do nothing
-        return;
-    }
 
-    // GCS failsafe event has occurred
+    } else if (last_gcs_update_ms > FS_GCS_TIMEOUT_MS && failsafe.gcs) {
-    set_failsafe_gcs(true);
+        // Already in failsafe, do nothing
+
+    } else if (last_gcs_update_ms > FS_GCS_TIMEOUT_MS && !failsafe.gcs) {
+        // New GCS failsafe event, trigger events
+        set_failsafe_gcs(true);
+        failsafe_gcs_on_event();
+    }
+}
+
+// failsafe_gcs_on_event - actions to take when GCS contact is lost
+void Copter::failsafe_gcs_on_event(void)
+{
     AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_OCCURRED);
-
-    // clear overrides so that RC control can be regained with radio.
     RC_Channels::clear_overrides();
 
-    if (should_disarm_on_failsafe()) {
+    // convert the desired failsafe response to the Failsafe_Action enum
-        arming.disarm();
+    Failsafe_Action desired_action;
-    } else {
+    switch (g.failsafe_gcs) {
-        if (control_mode == Mode::Number::AUTO &&
+        case FS_GCS_DISABLED:
-            g.failsafe_gcs == FS_GCS_ENABLED_CONTINUE_MISSION) {
+            desired_action = Failsafe_Action_None;
-            // continue mission
+            break;
-        } else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL) {
+        case FS_GCS_ENABLED_ALWAYS_RTL:
-            set_mode_SmartRTL_or_RTL(ModeReason::GCS_FAILSAFE);
+        case FS_GCS_ENABLED_CONTINUE_MISSION:
-        } else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND) {
+            desired_action = Failsafe_Action_RTL;
-            set_mode_SmartRTL_or_land_with_pause(ModeReason::GCS_FAILSAFE);
+            break;
-        } else { // g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL
+        case FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL:
-            set_mode_RTL_or_land_with_pause(ModeReason::GCS_FAILSAFE);
+            desired_action = Failsafe_Action_SmartRTL;
-        }
+            break;
+        case FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND:
+            desired_action = Failsafe_Action_SmartRTL_Land;
+            break;
+        case FS_GCS_ENABLED_ALWAYS_LAND:
+            desired_action = Failsafe_Action_Land;
+            break;
+        default: // if an invalid parameter value is set, the fallback is RTL
+            desired_action = Failsafe_Action_RTL;
     }
+
+    // Conditions to deviate from FS_GCS_ENABLE parameter setting
+    if (!motors->armed()) {
+        desired_action = Failsafe_Action_None;
+        gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe");
+
+    } else if (should_disarm_on_failsafe()) {
+        // should immediately disarm when we're on the ground
+        arming.disarm();
+        desired_action = Failsafe_Action_None;
+        gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Disarming");
+
+    } else if (flightmode->is_landing() && ((battery.has_failsafed() && battery.get_highest_failsafe_priority() <= FAILSAFE_LAND_PRIORITY))) {
+        // Allow landing to continue when battery failsafe requires it (not a user option)
+        gcs().send_text(MAV_SEVERITY_WARNING, "GCS + Battery Failsafe - Continuing Landing");
+        desired_action = Failsafe_Action_Land;
+
+    } else if (flightmode->is_landing() && failsafe_option(FailsafeOption::CONTINUE_IF_LANDING)) {
+        // Allow landing to continue when FS_OPTIONS is set to continue landing
+        gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Continuing Landing");
+        desired_action = Failsafe_Action_Land;
+
+    } else if (control_mode == Mode::Number::AUTO && failsafe_option(FailsafeOption::GCS_CONTINUE_IF_AUTO)) {
+        // Allow mission to continue when FS_OPTIONS is set to continue mission
+        gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Continuing Auto Mode");
+        desired_action = Failsafe_Action_None;
+
+    } else if (failsafe_option(FailsafeOption::GCS_CONTINUE_IF_PILOT_CONTROL) && !flightmode->is_autopilot()) {
+        // should continue when in a pilot controlled mode because FS_OPTIONS is set to continue in pilot controlled modes
+        gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Continuing Pilot Control");
+        desired_action = Failsafe_Action_None;
+    } else {
+        gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe");
+    }
+
+    // Call the failsafe action handler
+    do_failsafe_action(desired_action, ModeReason::GCS_FAILSAFE);
 }
 
 // failsafe_gcs_off_event - actions to take when GCS contact is restored
 void Copter::failsafe_gcs_off_event(void)
 {
+    gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe Cleared");
     AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_RESOLVED);
 }
 
@@ -294,3 +354,34 @@ bool Copter::should_disarm_on_failsafe() {
             return ap.land_complete;
     }
 }
+
+
+void Copter::do_failsafe_action(Failsafe_Action action, ModeReason reason){
+
+    // Execute the specified desired_action
+    switch (action) {
+        case Failsafe_Action_None:
+            return;
+        case Failsafe_Action_Land:
+            set_mode_land_with_pause(reason);
+            break;
+        case Failsafe_Action_RTL:
+            set_mode_RTL_or_land_with_pause(reason);
+            break;
+        case Failsafe_Action_SmartRTL:
+            set_mode_SmartRTL_or_RTL(reason);
+            break;
+        case Failsafe_Action_SmartRTL_Land:
+            set_mode_SmartRTL_or_land_with_pause(reason);
+            break;
+        case Failsafe_Action_Terminate:
+#if ADVANCED_FAILSAFE == ENABLED
+            char battery_type_str[17];
+            snprintf(battery_type_str, 17, "%s battery", type_str);
+            g2.afs.gcs_terminate(true, battery_type_str);
+#else
+            arming.disarm();
+#endif
+    }
+}
+