From a7fe242e317d43f217e9d84180784ed1d54de8d8 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 23 Feb 2018 16:33:49 +1100 Subject: [PATCH] Copter: add option to disable RTL flight mode --- ArduCopter/APM_Config.h | 1 + ArduCopter/Copter.h | 2 ++ ArduCopter/config.h | 6 ++++++ ArduCopter/events.cpp | 2 ++ ArduCopter/mode.cpp | 2 ++ 5 files changed, 13 insertions(+) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index c254703f52..ad546791f7 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -28,6 +28,7 @@ //#define MODE_GUIDED_ENABLED DISABLED // disable guided mode support //#define MODE_LOITER_ENABLED DISABLED // disable loiter mode support //#define MODE_POSHOLD_ENABLED DISABLED // disable poshold mode support +//#define MODE_RTL_ENABLED DISABLED // disable rtl mode support //#define MODE_SMARTRTL_ENABLED DISABLED // disable smartrtl mode support //#define MODE_SPORT_ENABLED DISABLED // disable sport mode support diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 97d7d711e8..a173df9f02 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -978,7 +978,9 @@ private: #if MODE_POSHOLD_ENABLED == ENABLED ModePosHold mode_poshold; #endif +#if MODE_RTL_ENABLED == ENABLED ModeRTL mode_rtl; +#endif #if FRAME_CONFIG == HELI_FRAME ModeStabilize_Heli mode_stabilize; #else diff --git a/ArduCopter/config.h b/ArduCopter/config.h index e040b56f67..0fcdc63517 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -297,6 +297,12 @@ # define MODE_POSHOLD_ENABLED ENABLED #endif +////////////////////////////////////////////////////////////////////////////// +// RTL - Return To Launch +#ifndef MODE_RTL_ENABLED +# define MODE_RTL_ENABLED ENABLED +#endif + ////////////////////////////////////////////////////////////////////////////// // SmartRTL - allows vehicle to retrace a (loop-eliminated) breadcrumb home #ifndef MODE_SMARTRTL_ENABLED diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index aaadb2811d..39dac52b12 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -189,8 +189,10 @@ void Copter::failsafe_terrain_on_event() if (should_disarm_on_failsafe()) { init_disarm_motors(); +#if MODE_RTL_ENABLED == ENABLED } else if (control_mode == RTL) { mode_rtl.restart_without_terrain(); +#endif } else { set_mode_RTL_or_land_with_pause(MODE_REASON_TERRAIN_FAILSAFE); } diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index d75b91f823..84b9c3a0df 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -76,9 +76,11 @@ Copter::Mode *Copter::mode_from_mode_num(const uint8_t mode) ret = &mode_land; break; +#if MODE_RTL_ENABLED == ENABLED case RTL: ret = &mode_rtl; break; +#endif #if MODE_DRIFT_ENABLED == ENABLED case DRIFT: