From 1474bf6fc35652238b7bdff1636f2bb09b3c673f Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov <goldartt@gmail.com>
Date: Sun, 16 Jun 2019 17:08:01 +0300
Subject: [PATCH] Modify flip function to work better in aruco_map frame with
 both Clever 3 & 4

(cherry picked from commit a5021d4b6e133c04a16732cdaa9e7baf089c221a)
---
 Drone/FlightLib/FlightLib.py | 8 ++++----
 Drone/copter_client.py       | 2 +-
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/Drone/FlightLib/FlightLib.py b/Drone/FlightLib/FlightLib.py
index 334caeb7..b08e59b1 100644
--- a/Drone/FlightLib/FlightLib.py
+++ b/Drone/FlightLib/FlightLib.py
@@ -359,10 +359,10 @@ def takeoff(z=Z_TAKEOFF, speed=SPEED_TAKEOFF, frame_id='body', freq=FREQUENCY,
     #print("Takeoff succeeded!")
     return 'success'
 
-def flip(min_z = FLIP_MIN_Z): #TODO Flip in different directions
+def flip(min_z = FLIP_MIN_Z, frame_id = FRAME_ID): #TODO Flip in different directions
     logger.info("Flip started!")
 
-    start_telemetry = get_telemetry()  # memorize starting position
+    start_telemetry = get_telemetry(frame_id=frame_id)  # memorize starting position
     
     if start_telemetry.z < min_z - TOLERANCE:
         logger.warning("Can't do flip! Flip failed!")
@@ -377,9 +377,9 @@ def flip(min_z = FLIP_MIN_Z): #TODO Flip in different directions
         while True:
             telem = get_telemetry()
 
-            if -math.pi + 0.1 < telem.roll < -0.2:
+            if abs(telem.roll) > math.pi/2:
                 break
 
         logger.info('Flip succeeded!')
         #print('Flip succeeded!')
-        navto(x=start_telemetry.x, y=start_telemetry.y, z=start_telemetry.z, yaw=start_telemetry.yaw)   # finish flip
+        navto(x=start_telemetry.x, y=start_telemetry.y, z=start_telemetry.z, yaw=start_telemetry.yaw, frame_id=frame_id)   # finish flip
diff --git a/Drone/copter_client.py b/Drone/copter_client.py
index facecb45..8f2bbdc9 100644
--- a/Drone/copter_client.py
+++ b/Drone/copter_client.py
@@ -108,7 +108,7 @@ def _command_emergency_led_fill(**kwargs):
 
 @messaging.message_callback("flip")
 def _copter_flip():
-    FlightLib.flip()
+    FlightLib.flip(frame_id=client.active_client.FRAME_ID)
 
 @messaging.message_callback("takeoff")
 def _command_takeoff(**kwargs):