Plane: make GUIDED behaviour match copter

we now have to be in GUIDED mode to accept a new GUIDED WP. When
entering GUIDED mode start loitering about the current position

ArduPlane/GCS_Mavlink.pde

@@ -960,6 +960,10 @@ GCS_MAVLINK::data_stream_send(void)
  */
 void GCS_MAVLINK::handle_guided_request(AP_Mission::Mission_Command &cmd)
 {
+    if (control_mode != GUIDED) {
+        // only accept position updates when in GUIDED mode
+        return;
+    }
     guided_WP_loc = cmd.content.location;
     
     // add home alt if needed
@@ -968,9 +972,6 @@ void GCS_MAVLINK::handle_guided_request(AP_Mission::Mission_Command &cmd)
         guided_WP_loc.flags.relative_alt = 0;
     }
 
-    set_mode(GUIDED);
-
-    // make any new wp uploaded instant (in case we are already in Guided mode)
     set_guided_WP();
 }
 

ArduPlane/commands.pde

@@ -111,11 +111,6 @@ static void init_home()
     // Save prev loc
     // -------------
     next_WP_loc = prev_WP_loc = home;
-
-    // Load home for a default guided_WP
-    // -------------
-    guided_WP_loc = home;
-    guided_WP_loc.alt += g.RTL_altitude_cm;
 }
 
 /*

ArduPlane/system.pde

@@ -410,6 +410,11 @@ static void set_mode(enum FlightMode mode)
     case GUIDED:
         auto_throttle_mode = true;
         guided_throttle_passthru = false;
+        /*
+          when entering guided mode we set the target as the current
+          location. This matches the behaviour of the copter code
+        */
+        guided_WP_loc = current_loc;
         set_guided_WP();
         break;
     }
@@ -442,6 +447,7 @@ static bool mavlink_set_mode(uint8_t mode)
     case AUTOTUNE:
     case FLY_BY_WIRE_B:
     case CRUISE:
+    case GUIDED:
     case AUTO:
     case RTL:
     case LOITER: