From 1dc18ec85a3342f45783d748447d08494e708f8d Mon Sep 17 00:00:00 2001 From: MatthewHampsey Date: Thu, 15 Sep 2022 13:51:06 +1000 Subject: [PATCH] Plane: reset yaw rate PID for scripting --- ArduPlane/Attitude.cpp | 4 ++++ ArduPlane/commands_logic.cpp | 2 ++ 2 files changed, 6 insertions(+) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index ba3315f577..6c108f0d69 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -352,6 +352,10 @@ void Plane::stabilize_yaw(float speed_scaler) now calculate steering_control.rudder for the rudder */ calc_nav_yaw_coordinated(speed_scaler); + /* + When not running the yaw rate controller, we need to reset the rate + */ + yawController.reset_rate_PID(); } diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 4465ca392b..986f8a3eb6 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -1159,10 +1159,12 @@ void Plane::do_nav_script_time(const AP_Mission::Mission_Command& cmd) nav_scripting.done = false; nav_scripting.id++; nav_scripting.start_ms = AP_HAL::millis(); + nav_scripting.current_ms = 0; // start with current roll rate, pitch rate and throttle nav_scripting.roll_rate_dps = plane.rollController.get_pid_info().target; nav_scripting.pitch_rate_dps = plane.pitchController.get_pid_info().target; + nav_scripting.yaw_rate_dps = degrees(ahrs.get_gyro().z); nav_scripting.throttle_pct = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle); }