diff --git a/ArduCopter/mode_smart_rtl.cpp b/ArduCopter/mode_smart_rtl.cpp index 15e45e48b8..9d111ce676 100644 --- a/ArduCopter/mode_smart_rtl.cpp +++ b/ArduCopter/mode_smart_rtl.cpp @@ -75,6 +75,15 @@ void Copter::ModeSmartRTL::wait_cleanup_run() void Copter::ModeSmartRTL::path_follow_run() { + float target_yaw_rate = 0.0f; + if (!copter.failsafe.radio) { + // get pilot's desired yaw rate + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + if (!is_zero(target_yaw_rate)) { + auto_yaw.set_mode(AUTO_YAW_HOLD); + } + } + // if we are close to current target point, switch the next point to be our target. if (wp_nav->reached_wp_destination()) { Vector3f next_point; @@ -103,7 +112,7 @@ void Copter::ModeSmartRTL::path_follow_run() // call attitude controller if (auto_yaw.mode() == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate); } else { // roll, pitch from waypoint controller, yaw heading from auto_heading() attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true);