From 4b7da42a6e797253772181052b0bde96680865e0 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 1 Jun 2023 09:25:09 +0200 Subject: [PATCH] FWPositionControl: navigateWaypoints: fix logic if already past waypoint and need to turn back (#21642) Signed-off-by: Silvan Fuhrer --- .../FixedwingPositionControl.cpp | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 33f65d2090..da1938ffc6 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -2847,31 +2847,36 @@ void FixedwingPositionControl::navigateWaypoints(const Vector2f &waypoint_A, con // BUT no arbitrary max approach angle, approach entirely determined by generated // bearing vectors - Vector2f vector_A_to_B = waypoint_B - waypoint_A; - Vector2f vector_A_to_vehicle = vehicle_pos - waypoint_A; + const Vector2f vector_A_to_B = waypoint_B - waypoint_A; + const Vector2f vector_B_to_A = -vector_A_to_B; + const Vector2f vector_A_to_vehicle = vehicle_pos - waypoint_A; + const Vector2f vector_B_to_vehicle = vehicle_pos - waypoint_B; + Vector2f desired_path = vector_A_to_B; // this is the default path tangent, but is overridden below if (vector_A_to_B.norm() < FLT_EPSILON) { // the waypoints are on top of each other and should be considered as a // single waypoint, fly directly to it if (vector_A_to_vehicle.norm() > FLT_EPSILON) { - vector_A_to_B = -vector_A_to_vehicle; + desired_path = -vector_A_to_vehicle; } else { // Fly to a point and on it. Stay to the current control. Do not update the npfg library to get last output. return; } - } else if ((vector_A_to_B.dot(vector_A_to_vehicle) < -FLT_EPSILON)) { // we are in front of waypoint A, fly directly to it until we are within switch distance. - if (vector_A_to_vehicle.norm() > _npfg.switchDistance(500.0f)) { - vector_A_to_B = -vector_A_to_vehicle; + desired_path = -vector_A_to_vehicle; } + + } else if (vector_B_to_A.dot(vector_B_to_vehicle) < -FLT_EPSILON) { + // we are behind waypoint B, fly directly to it + desired_path = -vector_B_to_vehicle; } // track the line segment - Vector2f unit_path_tangent{vector_A_to_B.normalized()}; + Vector2f unit_path_tangent{desired_path.normalized()}; _target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0)); _closest_point_on_path = waypoint_A + vector_A_to_vehicle.dot(unit_path_tangent) * unit_path_tangent; _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, waypoint_A, 0.0f);