AP_Plane: implement external control method set_path_position_tangent…

…_and_curvature

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

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

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