From eb20c90e4a27f8493dbf0758618e13f81c418e99 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
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();