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();