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); }