AP_L1_Control: add method update_path

- Implement method update_path - Update comment Signed-off-by: Rhys Mainwaring <[email protected]>
ArduPilot · Feb 26, 2024 · 348185a · 348185a
1 parent 0e7f307
commit 348185a
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 0 deletions.
diff --git 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
@@ -54,6 +54,7 @@ class AP_L1_Control : public AP_Navigation {
     void update_loiter(const class Location &center_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