controls SHOULD work

1 files changed, 130 insertions, 19 deletions

diff --git a/server.py b/server.py
index 60e03df..4233553 100644
--- a/server.py
+++ b/server.py
@@ -6,6 +6,10 @@ import socket
 import sys
 import threading
 import time
+import struct
+
+def putStatusText(img, text, pos, activated):
+    cv2.putText(img, text, pos, cv2.FONT_HERSHEY_PLAIN, 1, (0,0,255) if activated else (127,127,127), 2 if activated else 1)
 
 def encode_int(i):
     i = int(i)
@@ -22,15 +26,22 @@ class ServerThread(threading.Thread):
             try:
                 print >>sys.stderr, 'connection from', client_address
 
-                # Receive the data in small chunks and retransmit it
                 while True:
                     data = connection.recv(16)
                     if data:
                         if data=="get\n":
                             lock.acquire()
-                            framestr = frame.tostring()
-                            lenframestr=len(framestr)
-                            connection.sendall(encode_int(lenframestr)+framestr);
+                            framestr = global_frame.tostring()
+                            lenframestr=len(global_framestr)
+                            connection.sendall(struct.pack(">i",lenframestr)+framestr+struct.pack("@dddd", global_phi, global_theta, global_psi, global_batt));
+                            lock.release()
+                        elif data[0:3] == "fly" and data[-1]=="\n":
+                            values = data[3:-1].split()
+                            lock.acquire()
+                            global_cmd_x = float(values[0])
+                            global_cmd_y = float(values[1])
+                            global_cmd_z = float(values[2])
+                            global_cmd_rot = float(values[3])
                             lock.release()
                     else:
                         print >>sys.stderr, 'no more data from', client_address
@@ -62,6 +73,7 @@ sock.bind(server_address)
 # Listen for incoming connections
 sock.listen(1)
 
+no_flight = False
 
 try:
     pygame.init()
@@ -70,7 +82,12 @@ try:
     js.init()
     js_angle_shift = 0.0
 except:
-    print "meeeeh"
+    print "no joystick! disabling flight controls"
+    no_flight = True
+
+manual_override_xy = True
+manual_override_z = True
+manual_override_rot = True
 
 
 drone = libardrone.ARDrone(True, True)
@@ -80,31 +97,125 @@ drone.reset()
 serverthread=ServerThread()
 lock=threading.Lock()
 
-cap = cv2.VideoCapture("/home/flo/kruschkram/out2.avi")
-status, frame = cap.read()
-
 
 writer = cv2.VideoWriter("flight.avi",cv2.VideoWriter_fourcc(*'MP42'),25,(1280,720),1)
 logfile = open("flight.log", "w")
 
 
-
+global_phi = 0.
+global_theta = 0.
+global_psi = 0.
+global_batt = 0.
+global_frame =  None
 
 serverthread.start()
 while True:
-    print "hello world :)"
+    if no_flight == False:
+        btn_leftshoulder =  js.get_button(4) or js.get_button(6)
+        btn_rightshoulder = js.get_button(5) or js.get_button(7)
+        btn_thumb = js.get_button(0) or js.get_button(1) or js.get_button(2) or js.get_button(3)
+        btn_all = js.get_button(0) and js.get_button(1) and js.get_button(2) and js.get_button(3)
+        btn_readjust = js.get_button(10)
+
+        if btn_thumb:
+            drone.land()
+            manual_override_xy = True
+            manual_override_z = True
+            manual_override_rot = True
+        if btn_leftshoulder and btn_rightshoulder and js.get_button(10):
+            drone.takeoff()
+            manual_override_xy = True
+            manual_override_z = True
+            manual_override_rot = True
+        if btn_all:
+            drone.reset()
+            manual_override_xy = True
+            manual_override_z = True
+            manual_override_rot = True
+        if btn_readjust:
+            js_angle_shift = drone.navdata.get(0, dict()).get('psi',0)
+
+        rel_angle = (drone.navdata.get(0, dict()).get('psi',0) - js_angle_shift)/180.*math.pi
+
+        js_x = js.get_axis(0)
+        js_y = js.get_axis(1)
+        js_z = -js.get_axis(4)
+        js_rot = js.get_axis(3)
+        js_radius =  math.sqrt(js_x**2 + js_y**2)
+        if btn_leftshoulder==0:
+            js_x, js_y =  ( js_x * cos(rel_angle) + js_y * sin(rel_angle) )   ,  ( -js_x * sin(rel_angle) + js_y * cos(rel_angle) )
+        
+        js_hover = (btn_rightshoulder==0 and (js_radius <= 0.01))
+
+        if (js_radius > OVERRIDE_THRESHOLD): manual_override_xy = True
+        if (abs(js_z) > OVERRIDE_THRESHOLD): manual_override_z = True
+        if (abs(js_rot) > OVERRIDE_THRESHOLD): manual_override_rot = True
+
+        if manual_override_xy:
+            actual_hover, actual_x, actual_y = js_hover, js_x, js_y
+        else:
+            actual_hover, actual_x, actual_y = global_cmd_hover, global_cmd_x, global_cmd_y
+
+        if manual_override_z:
+            actual_z = js_z
+        else:
+            actual_z = global_cmd_z
+
+        if manual_override_rot:
+            actual_rot = js_rot
+        else:
+            actual_rot = global_cmd_rot
+
+        drone.move_freely( not actual_hover  , actual_x, actual_y, actual_z, actual_rot) 
+    
+    
+    
     lock.acquire()
     rawimg = drone.get_image()
-    frame = cv2.cvtColor(rawimg, cv2.COLOR_BGR2RGB)
-    phi = drone.navdata.get(0, dict()).get('phi',1337)
-    theta = drone.navdata.get(0, dict()).get('theta',1337)
-    psi = drone.navdata.get(0, dict()).get('psi',1337)
+    global_frame = cv2.cvtColor(rawimg, cv2.COLOR_BGR2RGB)
+    global_phi = drone.navdata.get(0, dict()).get('phi',1337)
+    global_theta = drone.navdata.get(0, dict()).get('theta',1337)
+    global_psi = drone.navdata.get(0, dict()).get('psi',1337)
+    global_batt = drone.navdata.get(0, dict()).get('batt',1337)
     lock.release()
 
-    cv2.imshow("frame", frame)
-    writer.write(frame)
-    logfile.write(str(phi)+"\t"+str(theta)+"\t"+str(psi)+"\n")
-    print phi,theta,psi
+    smallframe = cv2.resize(global_frame, (640,480))
+    cv2.rectangle(smallframe, (0,0), (640,30), (255,255,255), -1)
+    cv2.putText(smallframe, "override", (0,20), cv2.FONT_HERSHEY_PLAIN, 1, (255,0,0))
+    putStatusText(smallframe, "XY", (100,20), manual_override_xy)
+    putStatusText(smallframe, "height", (200,20), manual_override_z)
+    putStatusText(smallframe, "rotation", (300,20), manual_override_rot)
+    #cv2.putText(smallframe, "XY", (100,20), cv2.FONT_HERSHEY_PLAIN, 1, (0,0,255) if manual_override_xy else (127,127,127))
+    #cv2.putText(smallframe, "height", (250,20), cv2.FONT_HERSHEY_PLAIN, 1, (0,0,255)   f manual_override_z else (127,127,127))
+    #cv2.putText(smallframe, "rotation", (400,20), cv2.FONT_HERSHEY_PLAIN, 1, (0,0,255) if manual_override_rot else (127,127,127))
+    cv2.imshow("frame", smallframe)
+    writer.write(global_frame)
+    logfile.write(str(global_phi)+"\t"+str(global_theta)+"\t"+str(global_psi)+"\n")
+    logfile.flush()
+
+
+
+    key = cv2.waitKey(10) & 0xFF
+
+    if key == ord("t"):
+        drone.trim()
+
+    if key == ord("z"):
+        drone.set_max_vz(750.0000)
+        drone.set_max_rotspeed(1.0)
+        drone.set_max_angle(0.1)
+        print "slow"
+    elif key == ord("a"):
+        drone.set_max_vz(10000.0000)
+        drone.set_max_rotspeed(10)
+        drone.set_max_angle(0.6)
+        print "fast"
+    
+    if key == ord("1"):
+        manual_override_xy = False
+    elif key == ord("2"):
+        manual_override_z = False
+    elif key == ord("3"):
+        manual_override_rot = False
 
-    cv2.waitKey(50)