From ce0babc8f15f100a47523ed422532e9dbbf830b4 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 9 Nov 2024 15:29:57 +0000 Subject: [PATCH] Plane: QuadPlane: Clear pilot corrections on mode change to avoid getting stuck in QLand --- ArduPlane/mode_qland.cpp | 1 - ArduPlane/quadplane.cpp | 5 +++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/ArduPlane/mode_qland.cpp b/ArduPlane/mode_qland.cpp index f3a08aabde..e59e3c9a50 100644 --- a/ArduPlane/mode_qland.cpp +++ b/ArduPlane/mode_qland.cpp @@ -9,7 +9,6 @@ bool ModeQLand::_enter() quadplane.throttle_wait = false; quadplane.setup_target_position(); poscontrol.set_state(QuadPlane::QPOS_LAND_DESCEND); - poscontrol.pilot_correction_done = false; quadplane.last_land_final_agl = plane.relative_ground_altitude(plane.g.rangefinder_landing); quadplane.landing_detect.lower_limit_start_ms = 0; quadplane.landing_detect.land_start_ms = 0; diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index e7fc346758..87be3e1160 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -4596,6 +4596,11 @@ void QuadPlane::mode_enter(void) poscontrol.last_velocity_match_ms = 0; poscontrol.set_state(QuadPlane::QPOS_NONE); + // Clear any pilot corrections + poscontrol.pilot_correction_done = false; + poscontrol.pilot_correction_active = false; + poscontrol.target_vel_cms.zero(); + // clear guided takeoff wait on any mode change, but remember the // state for special behaviour guided_wait_takeoff_on_mode_enter = guided_wait_takeoff;