From f5aaa1a8ccdc5756ead56272772d4da41a4e33eb Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Tue, 27 Feb 2024 15:05:41 +0000 Subject: [PATCH] AP_Plane: implement external control method set_path_position_tangent_and_curvature Signed-off-by: Rhys Mainwaring --- ArduPlane/AP_ExternalControl_Plane.cpp | 20 ++++++++++++++++++++ ArduPlane/AP_ExternalControl_Plane.h | 7 +++++++ 2 files changed, 27 insertions(+) diff --git a/ArduPlane/AP_ExternalControl_Plane.cpp b/ArduPlane/AP_ExternalControl_Plane.cpp index c5afabf5843216..47bad935faabcd 100644 --- a/ArduPlane/AP_ExternalControl_Plane.cpp +++ b/ArduPlane/AP_ExternalControl_Plane.cpp @@ -19,4 +19,24 @@ bool AP_ExternalControl_Plane::set_global_position(const Location& loc) return plane.set_target_location(loc); } +bool AP_ExternalControl_Plane::set_path_position_tangent_and_curvature(const Location &position_on_path, + const Vector2f unit_path_tangent, const float path_curvature, const bool direction_is_ccw) +{ + //! @todo(srmainwaring) consolidate duplicate code in GCS_MAVLINK_Plane::handle_set_position_target_global_int. + + // only accept position updates when in GUIDED mode. + if (!plane.control_mode->is_guided_mode()) { + return false; + } + + // update guided path + plane.mode_guided.handle_guided_path_request( + position_on_path, unit_path_tangent, path_curvature, direction_is_ccw); + + // update adjust_altitude_target immediately rather than wait for the scheduler. + plane.adjust_altitude_target(); + + return true; +} + #endif // AP_EXTERNAL_CONTROL_ENABLED diff --git a/ArduPlane/AP_ExternalControl_Plane.h b/ArduPlane/AP_ExternalControl_Plane.h index 71308780d55fdb..5c1c993cea0f58 100644 --- a/ArduPlane/AP_ExternalControl_Plane.h +++ b/ArduPlane/AP_ExternalControl_Plane.h @@ -16,6 +16,13 @@ class AP_ExternalControl_Plane : public AP_ExternalControl { Sets the target global position for a loiter point. */ bool set_global_position(const Location& loc) override WARN_IF_UNUSED; + + /* + Set the target setpoint for path guidance: the closest position on the + path, the unit tangent to the path, and path curvature and direction. + */ + bool set_path_position_tangent_and_curvature(const Location &position_on_path, + Vector2f unit_path_tangent, const float path_curvature, const bool direction_is_ccw) override WARN_IF_UNUSED; }; #endif // AP_EXTERNAL_CONTROL_ENABLED