diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
index 994b522240..bae3ed679b 100644
--- a/ArduCopter/mode.h
+++ b/ArduCopter/mode.h
@@ -234,6 +234,8 @@ public:
 
         void set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds);
 
+        bool fixed_yaw_slew_finished() { return is_zero(_fixed_yaw_offset_cd); }
+
     private:
 
         float look_ahead_yaw();
diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp
index 56954e18cf..6b74938358 100644
--- a/ArduCopter/mode_auto.cpp
+++ b/ArduCopter/mode_auto.cpp
@@ -1871,13 +1871,11 @@ bool ModeAuto::verify_within_distance()
 // verify_yaw - return true if we have reached the desired heading
 bool ModeAuto::verify_yaw()
 {
-    // set yaw mode if it has been changed (the waypoint controller often retakes control of yaw as it executes a new waypoint command)
-    if (auto_yaw.mode() != AUTO_YAW_FIXED) {
-        auto_yaw.set_mode(AUTO_YAW_FIXED);
-    }
+    // make sure still in fixed yaw mode, the waypoint controller often retakes control of yaw as it executes a new waypoint command
+    auto_yaw.set_mode(AUTO_YAW_FIXED);
 
     // check if we are within 2 degrees of the target heading
-    return (fabsf(wrap_180_cd(ahrs.yaw_sensor-auto_yaw.yaw())) <= 200);
+    return auto_yaw.fixed_yaw_slew_finished() && (fabsf(wrap_180_cd(ahrs.yaw_sensor-auto_yaw.yaw())) <= 200);
 }
 
 // verify_nav_wp - check if we have reached the next way point