From 85a2e53483b8851e3c4ec0dddc05a94ee41841be Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Mon, 26 Feb 2024 11:30:51 +0000 Subject: [PATCH] AP_L1_Control: add method update_path - Implement method update_path - Update comment Signed-off-by: Rhys Mainwaring --- libraries/AP_L1_Control/AP_L1_Control.cpp | 28 +++++++++++++++++++++++ libraries/AP_L1_Control/AP_L1_Control.h | 1 + 2 files changed, 29 insertions(+) diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp index 2750c232298d0d..39128eac8b77a1 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.cpp +++ b/libraries/AP_L1_Control/AP_L1_Control.cpp @@ -515,3 +515,31 @@ void AP_L1_Control::update_level_flight(void) _data_is_stale = false; // status are correctly updated with current waypoint data } + +// update L1 control for path following +void AP_L1_Control::update_path(const class Location &position_on_path, Vector2f unit_path_tangent, float path_curvature, int8_t direction) { + //! @note initial implementation uses existing functions + if (!is_zero(path_curvature)) { + // moving along arc of circle - loiter about wp located at + // centre of curvature. + float radius_m = 1.0 / path_curvature; + auto center_wp = position_on_path; + Vector3p tangent_ned(unit_path_tangent.x, unit_path_tangent.y, 0.0); + Vector3p dn_ned(0.0, 0.0, 1.0); + auto ofs_ned = dn_ned.cross(tangent_ned) * radius_m * direction; + center_wp.offset(ofs_ned); + + update_loiter(center_wp, radius_m, direction); + } else { + // moving along a line segment - navigate to wp ahead of closest point + // in direction of path tangent + float ofs_m = 1000.0; + Vector3p ofs_ned(ofs_m * unit_path_tangent.x, + ofs_m * unit_path_tangent.y, 0.0); + auto prev_wp = position_on_path; + auto next_wp = position_on_path; + next_wp.offset(ofs_ned); + + update_waypoint(prev_wp, next_wp); + } +} diff --git a/libraries/AP_L1_Control/AP_L1_Control.h b/libraries/AP_L1_Control/AP_L1_Control.h index b775e5800a5a8a..b47fc07b7a5cc6 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.h +++ b/libraries/AP_L1_Control/AP_L1_Control.h @@ -54,6 +54,7 @@ class AP_L1_Control : public AP_Navigation { void update_loiter(const class Location ¢er_WP, float radius, int8_t loiter_direction) override; void update_heading_hold(int32_t navigation_heading_cd) override; void update_level_flight(void) override; + void update_path(const class Location &position_on_path, Vector2f unit_path_tangent, float path_curvature, int8_t direction) override; bool reached_loiter_target(void) override; // set the default NAVL1_PERIOD