From a0260b05b3b2c7f3c8b508f95fc2564d4d13c938 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Mon, 27 Sep 2021 20:28:39 +0100
Subject: [PATCH] Plane: handle_guided_request per mode

---
 ArduPlane/GCS_Mavlink.cpp | 15 +--------------
 ArduPlane/mode.h          |  8 +++++++-
 ArduPlane/mode_guided.cpp | 15 +++++++++++++++
 3 files changed, 23 insertions(+), 15 deletions(-)

diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp
index 00e12223e1..bb73dad2f7 100644
--- a/ArduPlane/GCS_Mavlink.cpp
+++ b/ArduPlane/GCS_Mavlink.cpp
@@ -604,20 +604,7 @@ const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = {
  */
 bool GCS_MAVLINK_Plane::handle_guided_request(AP_Mission::Mission_Command &cmd)
 {
-    if (plane.control_mode != &plane.mode_guided) {
-        // only accept position updates when in GUIDED mode
-        return false;
-    }
-    plane.guided_WP_loc = cmd.content.location;
-    
-    // add home alt if needed
-    if (plane.guided_WP_loc.relative_alt) {
-        plane.guided_WP_loc.alt += plane.home.alt;
-        plane.guided_WP_loc.relative_alt = 0;
-    }
-
-    plane.set_guided_WP();
-    return true;
+    return plane.control_mode->handle_guided_request(cmd.content.location);
 }
 
 /*
diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
index ecbaa36244..b121f8f23f 100644
--- a/ArduPlane/mode.h
+++ b/ArduPlane/mode.h
@@ -118,7 +118,10 @@ public:
 
     // method for mode specific target altitude profiles
     virtual bool update_target_altitude() { return false; }
-    
+
+    // handle a guided target request from GCS
+    virtual bool handle_guided_request(Location target_loc) { return false; }
+
 protected:
 
     // subclasses override this to perform checks before entering the mode
@@ -220,6 +223,9 @@ public:
 
     bool does_auto_throttle() const override { return true; }
 
+    // handle a guided target request from GCS
+    bool handle_guided_request(Location target_loc) override;
+
 protected:
 
     bool _enter() override;
diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp
index 5ffee4b5b5..aeac481989 100644
--- a/ArduPlane/mode_guided.cpp
+++ b/ArduPlane/mode_guided.cpp
@@ -42,3 +42,18 @@ void ModeGuided::navigate()
     // Zero indicates to use WP_LOITER_RAD
     plane.update_loiter(0);
 }
+
+bool ModeGuided::handle_guided_request(Location target_loc)
+{
+    plane.guided_WP_loc = target_loc;
+
+    // add home alt if needed
+    if (plane.guided_WP_loc.relative_alt) {
+        plane.guided_WP_loc.alt += plane.home.alt;
+        plane.guided_WP_loc.relative_alt = 0;
+    }
+
+    plane.set_guided_WP();
+
+    return true;
+}