Fix guess goal for forward perpendicular parking

diff --git a/src/psp.cc b/src/psp.cc
index 9021d260bf69e3f2fa5dcfb0877a7da164276bab..583643ac0d1ec5008f6b8f008a464fb9c740e05f 100644 (file)
--- 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(