diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 8aa454f7ac..a8b3ff62d8 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -604,7 +604,12 @@ void Plane::stabilize() 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); + float rudder = nav_scripting.rudder_offset_pct * 45; + if (nav_scripting.run_yaw_rate_controller) { + rudder += yawController.get_rate_out(nav_scripting.yaw_rate_dps, speed_scaler, false); + } else { + yawController.reset_I(); + } steering_control.rudder = rudder; } #endif diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index d2cf1e20f8..afc9c5cf98 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -522,6 +522,8 @@ private: float throttle_pct; uint32_t start_ms; uint32_t current_ms; + float rudder_offset_pct; + bool run_yaw_rate_controller; } nav_scripting; #endif @@ -1140,6 +1142,7 @@ private: // command throttle percentage and roll, pitch, yaw target // rates. For use with scripting controllers void set_target_throttle_rate_rpy(float throttle_pct, float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps) override; + void set_rudder_offset(float rudder_pct, bool run_yaw_rate_controller) override; bool nav_scripting_enable(uint8_t mode) override; #endif diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 037236deea..675ea7bfb4 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -1184,6 +1184,8 @@ bool Plane::verify_nav_script_time(const AP_Mission::Mission_Command& cmd) if (now - nav_scripting.start_ms > cmd.content.nav_script_time.timeout_s*1000U) { gcs().send_text(MAV_SEVERITY_INFO, "NavScriptTime timed out"); nav_scripting.enabled = false; + nav_scripting.rudder_offset_pct = 0; + nav_scripting.run_yaw_rate_controller = true; } } return !nav_scripting.enabled; @@ -1196,6 +1198,8 @@ bool Plane::nav_scripting_active(void) // set_target_throttle_rate_rpy has not been called from script in last 1000ms nav_scripting.enabled = false; nav_scripting.current_ms = 0; + nav_scripting.rudder_offset_pct = 0; + nav_scripting.run_yaw_rate_controller = true; gcs().send_text(MAV_SEVERITY_INFO, "NavScript time out"); } if (control_mode == &mode_auto && @@ -1240,6 +1244,13 @@ void Plane::set_target_throttle_rate_rpy(float throttle_pct, float roll_rate_dps nav_scripting.current_ms = AP_HAL::millis(); } +// support for rudder offset override in aerobatic scripting +void Plane::set_rudder_offset(float rudder_pct, bool run_yaw_rate_controller) +{ + nav_scripting.rudder_offset_pct = rudder_pct; + nav_scripting.run_yaw_rate_controller = run_yaw_rate_controller; +} + // enable NAV_SCRIPTING takeover in modes other than AUTO using script time mission commands bool Plane::nav_scripting_enable(uint8_t mode) {