From 20fe5bb98f9f7cb7d62982218abb4d9a2f48a8db Mon Sep 17 00:00:00 2001 From: Dylan Herman Date: Fri, 19 Jan 2018 08:44:02 -0500 Subject: [PATCH] Copter: add SmartRTL failsafe action Adds SmartRTL or RTL and SmartRTL or Land failsafe options for batt, throttle, and GCS failsafes --- ArduCopter/Copter.h | 2 ++ ArduCopter/Parameters.cpp | 6 ++--- ArduCopter/defines.h | 20 +++++++++------ ArduCopter/events.cpp | 51 ++++++++++++++++++++++++++++++++++----- 4 files changed, 63 insertions(+), 16 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 29c3aa5902..464232f085 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -698,6 +698,8 @@ private: void failsafe_terrain_on_event(); void gpsglitch_check(); void set_mode_RTL_or_land_with_pause(mode_reason_t reason); + void set_mode_SmartRTL_or_RTL(mode_reason_t reason); + void set_mode_SmartRTL_or_land_with_pause(mode_reason_t reason); bool should_disarm_on_failsafe(); void update_events(); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 6db9a1c44d..1520da3aa8 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -151,7 +151,7 @@ const AP_Param::Info Copter::var_info[] = { // @Param: FS_BATT_ENABLE // @DisplayName: Battery Failsafe Enable // @Description: Controls whether failsafe will be invoked when battery voltage or current runs low - // @Values: 0:Disabled,1:Land,2:RTL + // @Values: 0:Disabled,1:Land,2:RTL,3:SmartRTL or RTL,4:SmartRTL or Land // @User: Standard GSCALAR(failsafe_battery_enabled, "FS_BATT_ENABLE", FS_BATT_DISABLED), @@ -174,7 +174,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 + // @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 // @User: Standard GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_ENABLED_ALWAYS_RTL), @@ -272,7 +272,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 + // @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 // @User: Standard GSCALAR(failsafe_throttle, "FS_THR_ENABLE", FS_THR_ENABLED_ALWAYS_RTL), diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index a60d3d416a..f64256c9c8 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -474,20 +474,26 @@ enum DevOptions { #define ERROR_CODE_GPS_GLITCH 2 // 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 +#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 +#define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL 4 +#define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND 5 // Battery failsafe definitions (FS_BATT_ENABLE parameter) #define FS_BATT_DISABLED 0 // battery failsafe disabled #define FS_BATT_LAND 1 // switch to LAND mode on battery failsafe #define FS_BATT_RTL 2 // switch to RTL mode on battery failsafe +#define FS_BATT_SMARTRTL_OR_RTL 3 // switch to SmartRTL, if can't, switch to RTL +#define FS_BATT_SMARTRTL_OR_LAND 4 // switch to SmartRTL, if can't, swtich to LAND // 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_DISABLED 0 +#define FS_GCS_ENABLED_ALWAYS_RTL 1 +#define FS_GCS_ENABLED_CONTINUE_MISSION 2 +#define FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL 3 +#define FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND 4 // 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 3939ad4fa3..d743312d04 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -19,10 +19,14 @@ void Copter::failsafe_radio_on_event() } else if (control_mode == LAND && g.failsafe_battery_enabled == FS_BATT_LAND && failsafe.battery) { // continue landing } else { - if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) { - set_mode_land_with_pause(MODE_REASON_RADIO_FAILSAFE); - } else { + if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) { set_mode_RTL_or_land_with_pause(MODE_REASON_RADIO_FAILSAFE); + } else if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL) { + set_mode_SmartRTL_or_RTL(MODE_REASON_RADIO_FAILSAFE); + } else if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND) { + set_mode_SmartRTL_or_land_with_pause(MODE_REASON_RADIO_FAILSAFE); + } else { // g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND + set_mode_land_with_pause(MODE_REASON_RADIO_FAILSAFE); } } } @@ -54,9 +58,13 @@ void Copter::failsafe_battery_event(void) if (should_disarm_on_failsafe()) { init_disarm_motors(); } else { - if (g.failsafe_battery_enabled == FS_BATT_RTL || control_mode == AUTO) { + if (g.failsafe_battery_enabled == FS_BATT_SMARTRTL_OR_RTL) { + set_mode_SmartRTL_or_RTL(MODE_REASON_BATTERY_FAILSAFE); + } else if (g.failsafe_battery_enabled == FS_BATT_SMARTRTL_OR_LAND) { + set_mode_SmartRTL_or_land_with_pause(MODE_REASON_BATTERY_FAILSAFE); + } else if (g.failsafe_battery_enabled == FS_BATT_RTL || control_mode == AUTO) { set_mode_RTL_or_land_with_pause(MODE_REASON_BATTERY_FAILSAFE); - } else { + } else { // g.failsafe_battery_enabled == FS_BATT_LAND set_mode_land_with_pause(MODE_REASON_BATTERY_FAILSAFE); } } @@ -115,7 +123,11 @@ void Copter::failsafe_gcs_check() } else { if (control_mode == AUTO && g.failsafe_gcs == FS_GCS_ENABLED_CONTINUE_MISSION) { // continue mission - } else if (g.failsafe_gcs != FS_GCS_DISABLED) { + } else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL) { + set_mode_SmartRTL_or_RTL(MODE_REASON_GCS_FAILSAFE); + } else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND) { + set_mode_SmartRTL_or_land_with_pause(MODE_REASON_GCS_FAILSAFE); + } else { // g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL set_mode_RTL_or_land_with_pause(MODE_REASON_GCS_FAILSAFE); } } @@ -218,6 +230,33 @@ void Copter::set_mode_RTL_or_land_with_pause(mode_reason_t reason) } } +// set_mode_SmartRTL_or_land_with_pause - sets mode to SMART_RTL if possible or LAND with 4 second delay before descent starts +// this is always called from a failsafe so we trigger notification to pilot +void Copter::set_mode_SmartRTL_or_land_with_pause(mode_reason_t reason) +{ + // attempt to switch to SMART_RTL, if this failed then switch to Land + if (!set_mode(SMART_RTL, reason)) { + gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL Unavailable, Using Land Mode"); + set_mode_land_with_pause(reason); + } else { + AP_Notify::events.failsafe_mode_change = 1; + } +} + +// set_mode_SmartRTL_or_RTL - sets mode to SMART_RTL if possible or RTL if possible or LAND with 4 second delay before descent starts +// this is always called from a failsafe so we trigger notification to pilot +void Copter::set_mode_SmartRTL_or_RTL(mode_reason_t reason) +{ + // attempt to switch to SmartRTL, if this failed then attempt to RTL + // if that fails, then land + if (!set_mode(SMART_RTL, reason)) { + gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL Unavailable, Trying RTL Mode"); + set_mode_RTL_or_land_with_pause(reason); + } else { + AP_Notify::events.failsafe_mode_change = 1; + } +} + bool Copter::should_disarm_on_failsafe() { if (ap.in_arming_delay) { return true;