From 17648b5dff635a9f542f3a2403fa317b0e4f5547 Mon Sep 17 00:00:00 2001
From: Uwe Fechner <u.fechner-1@tudelft.nl>
Date: Sun, 21 Apr 2024 18:46:08 +0200
Subject: [PATCH] add timeout

---
 src/flightpathplanner2.jl | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/flightpathplanner2.jl b/src/flightpathplanner2.jl
index ace109f1..1b06e97e 100644
--- a/src/flightpathplanner2.jl
+++ b/src/flightpathplanner2.jl
@@ -18,6 +18,7 @@
     count         = 0
     finish::Bool  = false
     last_phi::Float64 = 0
+    timeout=0
 end
 
 function FlightPathPlanner(fpps::FPPSettings, fpca::FlightPathCalculator)
@@ -109,7 +110,7 @@ function on_new_data(fpp::FlightPathPlanner, depower, length, heading, height, t
     elseif state == FLY_LEFT  && phi > fpp.fpca._phi_sw 
         fpp.fpca.fig8 += 1            
         _switch(fpp, TURN_LEFT)
-    elseif state == TURN_LEFT && psi > deg2rad(180.0 - fpp.fpca._heading_offset)
+    elseif state == TURN_LEFT && (psi > deg2rad(180.0 + fpp.fpca._heading_offset) || fpp.timeout > 130)
         _switch(fpp, FLY_RIGHT)
     elseif state == FLY_RIGHT && phi >= phi_3
         if ! fpp.finish
@@ -119,7 +120,8 @@ function on_new_data(fpp::FlightPathPlanner, depower, length, heading, height, t
         _switch(fpp, UP_TURN_LEFT)
     elseif state == FLY_RIGHT && phi < -fpp.fpca._phi_sw # && 
         _switch(fpp, TURN_RIGHT)
-    elseif state == TURN_RIGHT && psi < deg2rad(180.0 + fpp.fpca._heading_offset) # && phi > -fpp._phi_sw - dphi
+    elseif state == TURN_RIGHT && (psi < deg2rad(180.0 + fpp.fpca._heading_offset) || fpp.timeout > 130) 
+        println("timeout TURN_RIGHT: $(fpp.timeout)")
         _switch(fpp, FLY_LEFT)
     elseif state == FLY_LEFT && phi <= -phi_3
         if ! fpp.finish
@@ -138,6 +140,7 @@ function on_new_data(fpp::FlightPathPlanner, depower, length, heading, height, t
         fpp.fpca.fig8 = 0
         _switch(fpp, POWER)
     end
+    fpp.timeout += 1
 end
 
 #  Call the related method of the flight path controller directly.
@@ -170,6 +173,7 @@ function _switch(fpp::FlightPathPlanner, state)
     if state == fpp._state
         return
     end
+    fpp.timeout = 0
     psi_dot_turn = fpp.fpca._omega / fpp.fpca._radius # desired turn rate during the turns
     sys_state = fpp.fpca._sys_state
     # see Table 5.3