From 48881eeb5517d2804f929aa3d56b68f958f73772 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 23 Mar 2022 14:34:22 +1100 Subject: [PATCH] Plane: fixed yaw on the ground when rudder disarming in AUTO when a quadplane touches down in an auto throttle mode the pilot may use rudder to disarm. The check for rudder disarm was only active in modes without auto-throttle. This expands it to all modes if the throttle has hit the lower limit --- ArduPlane/fence.cpp | 3 ++- ArduPlane/quadplane.cpp | 6 ++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index 1b8f15a317..d329719048 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -67,7 +67,8 @@ void Plane::fence_check() if (fence_act == AC_FENCE_ACTION_RTL_AND_LAND) { if (control_mode == &mode_auto && mission.get_in_landing_sequence_flag() && - (g.rtl_autoland == 1 || g.rtl_autoland == 2)) { + (g.rtl_autoland == RtlAutoland::RTL_THEN_DO_LAND_START || + g.rtl_autoland == RtlAutoland::RTL_IMMEDIATE_DO_LAND_START)) { // already landing return; } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 80b0b58f12..0bd05402d8 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1174,8 +1174,10 @@ float QuadPlane::get_pilot_input_yaw_rate_cds(void) const { bool manual_air_mode = plane.control_mode->is_vtol_man_throttle() && air_mode_active(); if (!manual_air_mode && - !is_positive(plane.get_throttle_input()) && !plane.control_mode->does_auto_throttle() && - plane.arming.get_rudder_arming_type() != AP_Arming::RudderArming::IS_DISABLED && !(inertial_nav.get_velocity_z_up_cms() < -0.5 * get_pilot_velocity_z_max_dn())) { + !is_positive(plane.get_throttle_input()) && + (!plane.control_mode->does_auto_throttle() || motors->limit.throttle_lower) && + plane.arming.get_rudder_arming_type() != AP_Arming::RudderArming::IS_DISABLED && + fabsf(inertial_nav.get_velocity_z_up_cms()) < 0.5 * get_pilot_velocity_z_max_dn()) { // the user may be trying to disarm return 0; }