From 1469342125d007e7b951e599956a5d1e3774bbe1 Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Mon, 22 Nov 2021 12:21:59 +0100 Subject: [PATCH] Handle negative path curvature for feedforward acceleration calculations --- src/lib/npfg/npfg.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/npfg/npfg.cpp b/src/lib/npfg/npfg.cpp index 52273542ba..41a2e516e9 100644 --- a/src/lib/npfg/npfg.cpp +++ b/src/lib/npfg/npfg.cpp @@ -456,8 +456,8 @@ float NPFG::lateralAccelFF(const Vector2f &unit_path_tangent, const Vector2f &gr // path frame curvature is the instantaneous curvature at our current distance // from the actual path (considering e.g. concentric circles emanating outward/inward) - const float path_frame_curvature = path_curvature / math::max(1.0f - path_curvature * signed_track_error, - path_curvature * MIN_RADIUS); + float path_frame_curvature = path_curvature / math::max(1.0f - path_curvature * signed_track_error, + fabsf(path_curvature) * MIN_RADIUS); // limit tangent ground speed to along track (forward moving) direction const float tangent_ground_speed = math::max(ground_vel.dot(unit_path_tangent), 0.0f);