diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 96c2764fe0..389f1cfa5b 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -594,7 +594,8 @@ private: Failsafe_Action_RTL = 2, Failsafe_Action_SmartRTL = 3, Failsafe_Action_SmartRTL_Land = 4, - Failsafe_Action_Terminate = 5 + Failsafe_Action_Terminate = 5, + Failsafe_Action_Auto_DO_LAND_START = 6, }; enum class FailsafeOption { @@ -731,6 +732,7 @@ private: void set_mode_RTL_or_land_with_pause(ModeReason reason); void set_mode_SmartRTL_or_RTL(ModeReason reason); void set_mode_SmartRTL_or_land_with_pause(ModeReason reason); + void set_mode_auto_do_land_start_or_RTL(ModeReason reason); bool should_disarm_on_failsafe(); void do_failsafe_action(Failsafe_Action action, ModeReason reason); diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index a45df46cdc..a283a26893 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -152,6 +152,7 @@ enum LoggingParameters { #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 +#define FS_THR_ENABLED_AUTO_RTL_OR_RTL 6 // GCS failsafe definitions (FS_GCS_ENABLE parameter) #define FS_GCS_DISABLED 0 @@ -160,6 +161,7 @@ enum LoggingParameters { #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 +#define FS_GCS_ENABLED_AUTO_RTL_OR_RTL 6 // 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 f5a97ddb86..e04f9d0807 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -33,6 +33,9 @@ void Copter::failsafe_radio_on_event() case FS_THR_ENABLED_ALWAYS_LAND: desired_action = Failsafe_Action_Land; break; + case FS_THR_ENABLED_AUTO_RTL_OR_RTL: + desired_action = Failsafe_Action_Auto_DO_LAND_START; + break; default: desired_action = Failsafe_Action_Land; } @@ -170,6 +173,9 @@ void Copter::failsafe_gcs_on_event(void) case FS_GCS_ENABLED_ALWAYS_LAND: desired_action = Failsafe_Action_Land; break; + case FS_GCS_ENABLED_AUTO_RTL_OR_RTL: + desired_action = Failsafe_Action_Auto_DO_LAND_START; + break; default: // if an invalid parameter value is set, the fallback is RTL desired_action = Failsafe_Action_RTL; } @@ -336,6 +342,18 @@ void Copter::set_mode_SmartRTL_or_RTL(ModeReason reason) } } +// Sets mode to Auto and jumps to DO_LAND_START, as set with AUTO_RTL param +// This can come from failsafe or RC option +void Copter::set_mode_auto_do_land_start_or_RTL(ModeReason reason) +{ + if (copter.mode_auto.jump_to_landing_sequence_auto_RTL(reason)) { + return; + } + + gcs().send_text(MAV_SEVERITY_WARNING, "Trying RTL Mode"); + set_mode_RTL_or_land_with_pause(reason); +} + bool Copter::should_disarm_on_failsafe() { if (ap.in_arming_delay) { return true; @@ -383,6 +401,10 @@ void Copter::do_failsafe_action(Failsafe_Action action, ModeReason reason){ #endif } break; + case Failsafe_Action_Auto_DO_LAND_START: + set_mode_auto_do_land_start_or_RTL(reason); + AP_Notify::events.failsafe_mode_change = 1; + break; } #if GRIPPER_ENABLED == ENABLED