From 56870ad7d6f867699294c1b74950d9ab1d8250ff Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 26 Nov 2021 10:18:13 +1100 Subject: [PATCH] Plane: use yaw rate controller in NAV_SCRIPT_TIME --- ArduPlane/Attitude.cpp | 4 ++++ ArduPlane/Plane.h | 1 + ArduPlane/commands_logic.cpp | 1 + 3 files changed, 6 insertions(+) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index da5fe31667..f80305b10c 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -519,6 +519,10 @@ void Plane::stabilize() const float elevator = pitchController.get_rate_out(nav_scripting.pitch_rate_dps, speed_scaler); SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator); + if (yawController.rate_control_enabled()) { + const float rudder = yawController.get_rate_out(nav_scripting.yaw_rate_dps, speed_scaler, false); + steering_control.rudder = rudder; + } #endif } else { if (allow_stick_mixing && g.stick_mixing == StickMixing::FBW && control_mode != &mode_stabilize) { diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 030ba5ebe7..a103224d5a 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -523,6 +523,7 @@ private: uint16_t id; float roll_rate_dps; float pitch_rate_dps; + float yaw_rate_dps; float throttle_pct; uint32_t start_ms; bool done; diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index d09ff5afad..9dbe16eb70 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -1188,6 +1188,7 @@ bool Plane::set_target_throttle_rate_rpy(float throttle_pct, float roll_rate_dps } nav_scripting.roll_rate_dps = roll_rate_dps; nav_scripting.pitch_rate_dps = pitch_rate_dps; + nav_scripting.yaw_rate_dps = yaw_rate_dps; nav_scripting.throttle_pct = throttle_pct; return true; }