Rover: Make guided operate the same way as Plane/Copter.

This is a safety change.  Lets say you have a GCS which is in
followme mode which is really GUIDED mode with continually updated
waypoints.  If the user then changes mode with the RC transmitter to
HOLD or anything else then the Rover should STOP listening to the
updated guided mode waypoints.  This is how Plane/Copter work.

APMrover2/GCS_Mavlink.cpp

@@ -789,9 +789,12 @@ GCS_MAVLINK::data_stream_send(void)
 
 void GCS_MAVLINK::handle_guided_request(AP_Mission::Mission_Command &cmd)
 {
-    rover.guided_WP = cmd.content.location;
+    if (rover.control_mode != GUIDED) {
+        // only accept position updates when in GUIDED mode
+        return;
+    }
         
-    rover.set_mode(GUIDED);
+    rover.guided_WP = cmd.content.location;
 
     // make any new wp uploaded instant (in case we are already in Guided mode)
     rover.rtl_complete = false;

APMrover2/system.cpp

@@ -306,6 +306,11 @@ void Rover::set_mode(enum mode mode)
 
         case GUIDED:
             rtl_complete = false;
+            /*
+              when entering guided mode we set the target as the current
+              location. This matches the behaviour of the copter code.
+            */
+            guided_WP = current_loc;
             set_guided_WP();
             break;
 
@@ -329,6 +334,7 @@ bool Rover::mavlink_set_mode(uint8_t mode)
     case HOLD:
     case LEARNING:
     case STEERING:
+    case GUIDED:
     case AUTO:
     case RTL:
         set_mode((enum mode)mode);