AP_NPFG: add method update_path

Signed-off-by: Rhys Mainwaring <[email protected]>

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;

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