From 3ffb57bccef7017e6eb78f9084d816e62076f58e Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Mon, 21 Aug 2023 13:57:10 +0200 Subject: [PATCH] FixedwingPositionControl: handle degenerate tangent setpoint in navigatePathTangent() --- src/modules/fw_pos_control/FixedwingPositionControl.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 4d615bfa27..831bd2ed4e 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -2926,12 +2926,16 @@ void FixedwingPositionControl::navigateLoiter(const Vector2f &loiter_center, con loiter_center + unit_vec_center_to_closest_pt * radius, path_curvature); } - void FixedwingPositionControl::navigatePathTangent(const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &position_setpoint, const matrix::Vector2f &tangent_setpoint, const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature) { + if (tangent_setpoint.norm() <= FLT_EPSILON) { + // degenerate case: no direction. maintain the last npfg command. + return; + } + const Vector2f unit_path_tangent{tangent_setpoint.normalized()}; _target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0)); _closest_point_on_path = position_setpoint;