From 32166e492b32affd8f79a11ec58cbdcd6e45cc21 Mon Sep 17 00:00:00 2001
From: Florian Jung <flo@windfisch.org>
Date: Mon, 5 Jan 2015 15:21:07 +0100
Subject: server can talk to drone

---
 server.py | 37 +++++++++++++++++++++++++++++++++++--
 1 file changed, 35 insertions(+), 2 deletions(-)

(limited to 'server.py')

diff --git a/server.py b/server.py
index 7627e9b..60e03df 100644
--- a/server.py
+++ b/server.py
@@ -1,3 +1,5 @@
+import libardrone.libardrone as libardrone
+import pygame
 import cv2
 import os
 import socket
@@ -61,17 +63,48 @@ sock.bind(server_address)
 sock.listen(1)
 
 
+try:
+    pygame.init()
+    pygame.joystick.init()
+    js=pygame.joystick.Joystick(0)
+    js.init()
+    js_angle_shift = 0.0
+except:
+    print "meeeeh"
+
+
+drone = libardrone.ARDrone(True, True)
+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")
+
+
+
+
 serverthread.start()
 while True:
     print "hello world :)"
-    time.sleep(1)
     lock.acquire()
-    status, frame = cap.read()
+    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)
     lock.release()
 
+    cv2.imshow("frame", frame)
+    writer.write(frame)
+    logfile.write(str(phi)+"\t"+str(theta)+"\t"+str(psi)+"\n")
+    print phi,theta,psi
+
+    cv2.waitKey(50)
+
-- 
cgit v1.2.3