diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 56c74573646b3..ef21ef09a352d 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -449,7 +449,7 @@ bool ModeAuto::wp_start(const Location& dest_loc) // initialise yaw // To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI - if (auto_yaw.mode() != AutoYaw::Mode::ROI) { + if (auto_yaw.mode() != AutoYaw::Mode::ROI && !(auto_yaw.mode() == AutoYaw::Mode::FIXED && copter.g.wp_yaw_behavior == WP_YAW_BEHAVIOR_NONE)) { auto_yaw.set_mode_to_default(false); } @@ -1795,7 +1795,7 @@ void ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd) // initialise yaw // To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI - if (auto_yaw.mode() != AutoYaw::Mode::ROI) { + if (auto_yaw.mode() != AutoYaw::Mode::ROI && !(auto_yaw.mode() == AutoYaw::Mode::FIXED && copter.g.wp_yaw_behavior == WP_YAW_BEHAVIOR_NONE)) { auto_yaw.set_mode_to_default(false); }