Copter: conditional yaw fix

waypoint command was setting auto_yaw_mode when it was run after the
do-cmd
yaw_look_at_heading was being set to current heading which was
overwriting the caller's desired heading

ArduCopter/commands_logic.pde

@@ -755,7 +755,13 @@ static bool verify_within_distance()
 // verify_yaw - return true if we have reached the desired heading
 static bool verify_yaw()
 {
-    if( labs(wrap_180_cd(ahrs.yaw_sensor-yaw_look_at_heading)) <= 200 ) {
+    // 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_LOOK_AT_HEADING) {
+        set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);
+    }
+
+    // check if we are within 2 degrees of the target heading
+    if (labs(wrap_180_cd(ahrs.yaw_sensor-yaw_look_at_heading)) <= 200) {
         return true;
     }else{
         return false;

ArduCopter/control_auto.pde

@@ -403,8 +403,8 @@ void set_auto_yaw_mode(uint8_t yaw_mode)
         break;
 
     case AUTO_YAW_LOOK_AT_HEADING:
-        // keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed
-        yaw_look_at_heading = ahrs.yaw_sensor;
+        // keep heading pointing in the direction held in yaw_look_at_heading
+        // caller should set the yaw_look_at_heading
         break;
 
     case AUTO_YAW_LOOK_AHEAD: