From ff126db1d37ff3dab8c7fb91536c1a51e07b5928 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 10 May 2015 14:28:02 +1000
Subject: [PATCH] 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 | 7 ++++---
 ArduPlane/commands.pde    | 5 -----
 ArduPlane/system.pde      | 6 ++++++
 3 files changed, 10 insertions(+), 8 deletions(-)

diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde
index 00610547dc..993430c2d4 100644
--- a/ArduPlane/GCS_Mavlink.pde
+++ b/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();
 }
 
diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde
index 9f2cc24554..df07802932 100644
--- a/ArduPlane/commands.pde
+++ b/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;
 }
 
 /*
diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde
index 808b9832b3..73fefb802d 100644
--- a/ArduPlane/system.pde
+++ b/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: