From 0cd1f605f6077a2cbbc6a1b3aff584b1e5d0a7cc Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 21 Jun 2022 00:14:45 +0100 Subject: [PATCH] Plane: Quadplane: always reset to QPOS_NONE on mode entry --- ArduPlane/mode_qrtl.cpp | 3 --- ArduPlane/quadplane.cpp | 1 + 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/ArduPlane/mode_qrtl.cpp b/ArduPlane/mode_qrtl.cpp index 138cf62406..0c4986066a 100644 --- a/ArduPlane/mode_qrtl.cpp +++ b/ArduPlane/mode_qrtl.cpp @@ -14,9 +14,6 @@ bool ModeQRTL::_enter() submode = SubMode::RTL; plane.prev_WP_loc = plane.current_loc; - // clear QPOS state - quadplane.poscontrol.set_state(QuadPlane::QPOS_NONE); - const int32_t RTL_alt_abs_cm = plane.home.alt + quadplane.qrtl_alt*100UL; if (quadplane.motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) { // VTOL motors are active, either in VTOL flight or assisted flight diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 4ff909ed64..68142b22a4 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -4137,6 +4137,7 @@ void QuadPlane::mode_enter(void) poscontrol.xy_correction.zero(); poscontrol.velocity_match.zero(); poscontrol.last_velocity_match_ms = 0; + poscontrol.set_state(QuadPlane::QPOS_NONE); // clear guided takeoff wait on any mode change, but remember the // state for special behaviour