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
This commit is contained in:
Andrew Tridgell 2022-04-03 09:13:25 +10:00
parent 283742edf1
commit 557c96b7f6
1 changed files with 6 additions and 8 deletions

View File

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