From aeed2b1a9df0e6105d4ebdb5e6a82b2b7bfb637f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 3 Apr 2022 09:13:25 +1000 Subject: [PATCH] Plane: fixed rudder control when ARMING_RUDDER != 2 when rudder disarm is disabled we should allow full yaw control regardless of throttle level. We should also only disable left yaw when throttle is at zero, as right yaw does not indicate pilot may be trying to disarm --- ArduPlane/quadplane.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index ffc715c033..63828b0c07 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1172,13 +1172,15 @@ float QuadPlane::landing_descent_rate_cms(float height_above_ground) */ float QuadPlane::get_pilot_input_yaw_rate_cds(void) const { + const auto rudder_in = plane.channel_rudder->get_control_in(); 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() || motors->limit.throttle_lower) && - plane.arming.get_rudder_arming_type() != AP_Arming::RudderArming::IS_DISABLED && + plane.arming.get_rudder_arming_type() == AP_Arming::RudderArming::ARMDISARM && + rudder_in < 0 && fabsf(inertial_nav.get_velocity_z_up_cms()) < 0.5 * get_pilot_velocity_z_max_dn()) { - // the user may be trying to disarm + // the user may be trying to disarm, disable pilot yaw control return 0; } @@ -1204,7 +1206,7 @@ float QuadPlane::get_pilot_input_yaw_rate_cds(void) const // must have a non-zero max yaw rate for scaling to work max_rate = (yaw_rate_max < 1.0f) ? 1 : yaw_rate_max; } - return plane.channel_rudder->get_control_in() * max_rate / 45; + return rudder_in * max_rate * (1/45.0); } /* @@ -1217,11 +1219,7 @@ float QuadPlane::get_desired_yaw_rate_cds(bool should_weathervane) // use bank angle to get desired yaw rate yaw_cds += desired_auto_yaw_rate_cds(); } - bool manual_air_mode = plane.control_mode->is_vtol_man_throttle() && air_mode_active(); - if (!is_positive(plane.get_throttle_input()) && !plane.control_mode->does_auto_throttle() && !manual_air_mode && !(inertial_nav.get_velocity_z_up_cms() < -0.5 * get_pilot_velocity_z_max_dn())) { - // the user may be trying to disarm - return 0; - } + // add in pilot input yaw_cds += get_pilot_input_yaw_rate_cds();