From 9ba4941aa769e62dd9479a2b4ccd356b03b968a1 Mon Sep 17 00:00:00 2001 From: Matt Lawrence Date: Sat, 21 Sep 2019 20:09:18 -0400 Subject: [PATCH] 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 | 12 ++ ArduCopter/Parameters.cpp | 47 +++++- ArduCopter/Parameters.h | 3 + ArduCopter/defines.h | 5 +- ArduCopter/events.cpp | 293 +++++++++++++++++++++++++------------- 5 files changed, 254 insertions(+), 106 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 94f34af4e8..4814f3415d 100644 --- a/ArduCopter/Copter.h +++ b/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(); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index b7bf9f3087..25a70de647 100644 --- a/ArduCopter/Parameters.cpp +++ b/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); +} diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 239ce91e1e..2df4d1970b 100644 --- a/ArduCopter/Parameters.h +++ b/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[]; diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index b36a7496ee..69341bc101 100644 --- a/ArduCopter/defines.h +++ b/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 diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index 4c91f239a1..2a56baece3 100644 --- a/ArduCopter/events.cpp +++ b/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) { - 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 - } + gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe"); } + + // 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) { - // we must run the failsafe checks if we are in failsafe - - // 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 { + // Bypass GCS failsafe checks if disabled or GCS never connected + if (g.failsafe_gcs == FS_GCS_DISABLED || failsafe.last_heartbeat_ms == 0) { 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 - if (last_gcs_update_ms < FS_GCS_TIMEOUT_MS) { - // check for recovery from gcs failsafe - if (failsafe.gcs) { - failsafe_gcs_off_event(); - set_failsafe_gcs(false); - } - return; - } + // Determine which event to trigger + if (last_gcs_update_ms < FS_GCS_TIMEOUT_MS && failsafe.gcs) { + // Recovery from a GCS failsafe + set_failsafe_gcs(false); + failsafe_gcs_off_event(); - // do nothing if gcs failsafe already triggered or motors disarmed - if (failsafe.gcs || !motors->armed()) { - return; - } + } else if (last_gcs_update_ms < FS_GCS_TIMEOUT_MS && !failsafe.gcs) { + // No problem, do nothing - // GCS failsafe event has occurred - set_failsafe_gcs(true); + } else if (last_gcs_update_ms > FS_GCS_TIMEOUT_MS && failsafe.gcs) { + // 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()) { - arming.disarm(); - } else { - if (control_mode == Mode::Number::AUTO && - g.failsafe_gcs == FS_GCS_ENABLED_CONTINUE_MISSION) { - // continue mission - } else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL) { - set_mode_SmartRTL_or_RTL(ModeReason::GCS_FAILSAFE); - } else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND) { - set_mode_SmartRTL_or_land_with_pause(ModeReason::GCS_FAILSAFE); - } else { // g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL - set_mode_RTL_or_land_with_pause(ModeReason::GCS_FAILSAFE); - } + // convert the desired failsafe response to the Failsafe_Action enum + Failsafe_Action desired_action; + switch (g.failsafe_gcs) { + case FS_GCS_DISABLED: + desired_action = Failsafe_Action_None; + break; + case FS_GCS_ENABLED_ALWAYS_RTL: + case FS_GCS_ENABLED_CONTINUE_MISSION: + desired_action = Failsafe_Action_RTL; + break; + case FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL: + 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 + } +} +