Fix forward for perpendicular parking

diff --git a/src/psp.cc b/src/psp.cc
index 9529383aa1c3bddaa873ddbef0b340ccaaa8713e..c64acd01c8dc7677c75febbd0b6333d7f574a2c1 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -21,16 +21,18 @@ bool PSPlanner::collide()
 
 bool PSPlanner::forward()
 {
-        double heading = this->ps().heading();
+        if (this->ps().parallel())
+                return false;
+        double heading = atan2(
+                this->ps().y2() - this->ps().y1(),
+                this->ps().x2() - this->ps().x1()
+        );
         while (heading < 0) heading += 2 * M_PI;
-        if (!this->ps().parallel())
-                heading -= M_PI / 2;
         double h = this->gc().h();
         while (h < 0) h += 2 * M_PI;
-        if (-0.00001 < heading - h && heading - h < 0.00001)
+        if (std::abs(heading - h) < M_PI / 4)
                 return true;
-        else
-                return false;
+        return false;
 }
 
 void PSPlanner::guess_gc()