From 4ad246c3bfb952d3b4d30c1abec08774fa5a6dc9 Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Tue, 21 Jul 2020 15:33:07 +0200 Subject: [PATCH] Fix guess goal for forward perpendicular parking --- src/psp.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/psp.cc b/src/psp.cc index 9021d26..583643a 100644 --- a/src/psp.cc +++ b/src/psp.cc @@ -107,8 +107,8 @@ void PSPlanner::guess_gc() this->ps().x2() - this->ps().x1() ); while (h < 0) h += 2 * M_PI; - x += this->gc().dr() * cos(h + M_PI); - y += this->gc().dr() * sin(h + M_PI); + x += 2 * this->gc().dr() * cos(h); + y += 2 * this->gc().dr() * sin(h); #else // Backward parking double entry_width = edist( -- 2.39.2