From a0260b05b3b2c7f3c8b508f95fc2564d4d13c938 Mon Sep 17 00:00:00 2001 From: Iampete1 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; +}