From d34c4b01f4cfb4e9f75bb7473ed30f1bb14020d0 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 26 Oct 2018 12:28:03 +1100 Subject: [PATCH] Copter: build return path in run() rather than init() This avoids attempting to build a return path if we don't currently have a home or origin --- ArduCopter/mode.h | 4 +++- ArduCopter/mode_rtl.cpp | 7 ++++++- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index a996cdeca7..c0eef6331d 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1027,7 +1027,9 @@ private: } rtl_path; // Loiter timer - Records how long we have been in loiter - uint32_t _loiter_start_time = 0; + uint32_t _loiter_start_time; + + bool path_built; }; diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 1844777729..f4cbf91ca3 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -19,7 +19,7 @@ bool Copter::ModeRTL::init(bool ignore_checks) } // initialise waypoint and spline controller wp_nav->wp_and_spline_init(); - build_path(!copter.failsafe.terrain); + path_built = false; climb_start(); return true; } @@ -39,6 +39,11 @@ void Copter::ModeRTL::restart_without_terrain() // should be called at 100hz or more void Copter::ModeRTL::run(bool disarm_on_land) { + if (!path_built) { + path_built = true; + build_path(!copter.failsafe.terrain); + } + // check if we need to move to next state if (_state_complete) { switch (_state) {