From f3b794f698052b27f914e58a330e7c862e546c5b Mon Sep 17 00:00:00 2001 From: Dylan Herman Date: Thu, 18 Jan 2018 13:19:50 -0500 Subject: [PATCH] Rover: Add SmartRTL failsafe action Add failsafe actions SmartRTL or RTL and SmartRTL or Hold --- APMrover2/Parameters.cpp | 2 +- APMrover2/failsafe.cpp | 12 ++++++++++++ 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index ef9c75e1ad..2363dbc1af 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -152,7 +152,7 @@ const AP_Param::Info Rover::var_info[] = { // @Param: FS_ACTION // @DisplayName: Failsafe Action // @Description: What to do on a failsafe event - // @Values: 0:Nothing,1:RTL,2:Hold + // @Values: 0:Nothing,1:RTL,2:Hold,3:SmartRTL or RTL,4:SmartRTL or Hold // @User: Standard GSCALAR(fs_action, "FS_ACTION", 2), diff --git a/APMrover2/failsafe.cpp b/APMrover2/failsafe.cpp index dbca59ed94..5a44987913 100644 --- a/APMrover2/failsafe.cpp +++ b/APMrover2/failsafe.cpp @@ -77,6 +77,18 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on) case 2: set_mode(mode_hold, MODE_REASON_FAILSAFE); break; + case 3: + if (!set_mode(mode_smartrtl, MODE_REASON_FAILSAFE)) { + if (!set_mode(mode_rtl, MODE_REASON_FAILSAFE)) { + set_mode(mode_hold, MODE_REASON_FAILSAFE); + } + } + break; + case 4: + if (!set_mode(mode_smartrtl, MODE_REASON_FAILSAFE)) { + set_mode(mode_hold, MODE_REASON_FAILSAFE); + } + break; } } }