Plane: handle_guided_request per mode

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);
 }
 
 /*

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;

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;
+}