From 63be15e0182bb727d9ff8f274c82f496bf16c3f8 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 4 Sep 2021 20:21:11 +0100 Subject: [PATCH] Plane: mode_qrtl: combine enter and init --- ArduPlane/mode_qrtl.cpp | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/ArduPlane/mode_qrtl.cpp b/ArduPlane/mode_qrtl.cpp index d73c3c430d..07a25f97ca 100644 --- a/ArduPlane/mode_qrtl.cpp +++ b/ArduPlane/mode_qrtl.cpp @@ -2,15 +2,6 @@ #include "Plane.h" bool ModeQRTL::_enter() -{ - return plane.mode_qstabilize._enter(); -} - - -/* - handle QRTL mode - */ -void ModeQRTL::init() { // use do_RTL() to setup next_WP_loc plane.do_RTL(plane.home.alt + quadplane.qrtl_alt*100UL); @@ -30,10 +21,11 @@ void ModeQRTL::init() int32_t to_alt; if (plane.current_loc.get_alt_cm(Location::AltFrame::ABSOLUTE,from_alt) && plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABSOLUTE,to_alt)) { poscontrol.slow_descent = from_alt > to_alt; - return; + return true; } // defualt back to old method poscontrol.slow_descent = (plane.current_loc.alt > plane.next_WP_loc.alt); + return true; } void ModeQRTL::update()