From 62754b82ca2c0900500d43b2e5773873d75ae9a3 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Fri, 23 Aug 2019 21:24:03 +0200
Subject: [PATCH] Implement naive fe_perpendicular

---
 src/psp.cc | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/src/psp.cc b/src/psp.cc
index 395deec..6cc5701 100644
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -266,7 +266,13 @@ void PSPlanner::fe_perpendicular()
         //
         //      Another approach could be testing angles from the
         //      beginning of the escape parkig slot maneuver.
-        return fer_perpendicular();
+        if (this->forward())
+                this->cc().sp(-0.01);
+        else
+                this->cc().sp(0.01);
+        while (!this->left())
+                this->cc().next();
+        return;
 }
 
 void PSPlanner::fer()
-- 
2.39.2