From 10fe4bfcef107f91bd8f72c4e3113e8d15e18545 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Mon, 26 Feb 2024 11:31:15 +0000 Subject: [PATCH] AP_NPFG: add method update_path Signed-off-by: Rhys Mainwaring --- libraries/AP_NPFG/AP_NPFG.cpp | 10 ++++++++++ libraries/AP_NPFG/AP_NPFG.h | 10 ++++++++++ 2 files changed, 20 insertions(+) diff --git a/libraries/AP_NPFG/AP_NPFG.cpp b/libraries/AP_NPFG/AP_NPFG.cpp index e80a7f5f9e480..96820e7fb7120 100644 --- a/libraries/AP_NPFG/AP_NPFG.cpp +++ b/libraries/AP_NPFG/AP_NPFG.cpp @@ -223,6 +223,16 @@ void AP_NPFG::update_level_flight(void) { // not implemented } +void AP_NPFG::update_path(const class Location &position_on_path, Vector2f unit_path_tangent, float path_curvature, int8_t direction) { + //! @note initial implementation uses existing functions + float radius_m = 0.0; + if (!is_zero(path_curvature)) { + radius_m = 1.0 / path_curvature; + } + set_path_tangent(_unit_path_tangent); + update_loiter(position_on_path, radius_m, direction); +} + bool AP_NPFG::reached_loiter_target(void) { // not implemented return false; diff --git a/libraries/AP_NPFG/AP_NPFG.h b/libraries/AP_NPFG/AP_NPFG.h index fe8f0b00627f3..00d948d2b24b6 100644 --- a/libraries/AP_NPFG/AP_NPFG.h +++ b/libraries/AP_NPFG/AP_NPFG.h @@ -101,6 +101,16 @@ class AP_NPFG : public AP_Navigation { // attitude/steering. void update_level_flight(void) override; + // update the internal state of the navigation controller when + // the vehicle has been commanded with a path following setpoint, which + // includes the closest point on the path, the unit tangent to the path, + // and the curvature. This is the step function for navigation control when + // path following. This function is called at regular intervals + // (typically 10Hz). The main flight code will call an output function + // (such as nav_roll_cd()) after this function to ask for the new required + // navigation attitude/steering. + void update_path(const class Location &position_on_path, Vector2f unit_path_tangent, float path_curvature, int8_t direction) override; + // return true if we have reached the target loiter location. This // may be a fuzzy decision, depending on internal navigation // parameters. For example the controller may return true only