From 56870ad7d6f867699294c1b74950d9ab1d8250ff Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
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;
 }