Copter: guided mode yaw fix

Nose of copter now points at next guided point if it is more than 10m
away

ArduCopter/GCS_Mavlink.pde

@@ -1729,11 +1729,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         }
 
         if(packet.current == 2) {                                               //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission
-            // switch to guided mode
+            // initiate guided mode
-            set_mode(GUIDED);
+            do_guided(&tell_command);
-
-            // set wp_nav's destination
-            wp_nav.set_destination(pv_location_to_vector(tell_command));
 
             // verify we recevied the command
             mavlink_msg_mission_ack_send(

ArduCopter/commands_logic.pde

@@ -843,6 +843,35 @@ static bool verify_nav_roi()
 //	Do (Now) commands
 /********************************************************************************/
 
+// do_guided - start guided mode
+// this is not actually a mission command but rather a 
+static void do_guided(const struct Location *cmd)
+{
+    bool first_time = false;
+    // switch to guided mode if we're not already in guided mode
+    if (control_mode != GUIDED) {
+        set_mode(GUIDED);
+        first_time = true;
+    }
+
+    // set wp_nav's destination
+    Vector3f pos = pv_location_to_vector(*cmd);
+    wp_nav.set_destination(pos);
+
+    // initialise wp_bearing for reporting purposes
+    wp_bearing = wp_nav.get_bearing_to_destination();
+
+    // point nose at next waypoint if it is more than 10m away
+    if (yaw_mode == YAW_LOOK_AT_NEXT_WP) {
+        // get distance to new location
+        wp_distance = wp_nav.get_distance_to_destination();
+        // set original_wp_bearing to point at next waypoint
+        if (wp_distance >= 1000 || first_time) {
+            original_wp_bearing = wp_bearing;
+        }
+    }
+}
+
 static void do_change_speed()
 {
     wp_nav.set_horizontal_velocity(command_cond_queue.p1 * 100);