From 38d7bcff6b3ba97b2d37cc3961b0736aabf4f96b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 24 Feb 2023 20:13:47 +1100 Subject: [PATCH] Plane: re-init throttle wait on quadplane arm and disarm this prevents yaw from rudder arming on 2nd flight --- ArduPlane/RC_Channel.cpp | 2 ++ ArduPlane/quadplane.cpp | 6 ++++++ 2 files changed, 8 insertions(+) diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index 42a2a85d96..e6a5ab6d4c 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/ArduPlane/RC_Channel.cpp @@ -314,6 +314,7 @@ bool RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const AuxSwit switch (ch_flag) { case AuxSwitchPos::HIGH: plane.quadplane.air_mode = AirMode::ON; + plane.quadplane.throttle_wait = false; break; case AuxSwitchPos::MIDDLE: break; @@ -364,6 +365,7 @@ bool RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const AuxSwit RC_Channel::do_aux_function_armdisarm(ch_flag); if (plane.arming.is_armed()) { plane.quadplane.air_mode = AirMode::ON; + plane.quadplane.throttle_wait = false; } break; diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 67d50f8984..8110ebcffc 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1360,6 +1360,12 @@ void QuadPlane::set_armed(bool armed) if (plane.control_mode == &plane.mode_guided) { guided_wait_takeoff = armed; } + + // re-init throttle wait on arm and disarm, to prevent rudder + // arming on 2nd flight causing yaw + if (!air_mode_active()) { + init_throttle_wait(); + } }