summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorFlorian Jung <flo@windfisch.org>2015-04-16 18:25:34 +0200
committerFlorian Jung <flo@windfisch.org>2015-04-16 18:25:34 +0200
commitc065db1f08e41646fd37ea34823f7768e6904bd0 (patch)
treeb4542e62d2b475bc90a660d1ea9ce9e9ac8b94cd
parentf303c0d49e3e3e5089052d797f3ab2e821c01636 (diff)
simple rotation commands
-rw-r--r--client2.cpp28
-rw-r--r--lib.cpp14
-rw-r--r--lib.h1
-rw-r--r--server.py26
-rw-r--r--server2.py5
5 files changed, 65 insertions, 9 deletions
diff --git a/client2.cpp b/client2.cpp
index 8d2e480..90372fa 100644
--- a/client2.cpp
+++ b/client2.cpp
@@ -528,6 +528,19 @@ float deadzone(float val, float dead)
return val;
}
+float saturate(float val, float sat)
+{
+ if (val < 0.)
+ {
+ if (val < -sat) val = -sat;
+ }
+ else
+ {
+ if (val > sat) val = sat;
+ }
+ return val;
+}
+
pthread_mutex_t my_mutex;
sem_t oculus_thread_go;
@@ -535,6 +548,7 @@ sem_t oculus_thread_go;
ModuloRingbuffer ringbuf_yaw_sensor_slow(40, -180,180);
ModuloRingbuffer ringbuf_pitch_sensor_slow(40, -180,180);
float yaw_cam_global, pitch_cam_global, roll_cam_global;
+float oculus_yaw_global;
Mat frame_global;
void* video_fetcher_thread(void* ptr)
@@ -553,7 +567,7 @@ void* video_fetcher_thread(void* ptr)
Ringbuffer delay_psi(DELAY_SIZE); // that's the amount the video lags behind
Ringbuffer delay_theta(DELAY_SIZE); // the sensor data
- int adjust_phi=10;
+ int adjust_phi=0;//10;
DroneConnection drone(SOCKETPATH);
@@ -680,8 +694,16 @@ void* video_fetcher_thread(void* ptr)
ringbuf_yaw_sensor_slow.put(navdata.psi);
ringbuf_pitch_sensor_slow.put(navdata.theta);
+
+ float oculus_yaw = oculus_yaw_global;
pthread_mutex_unlock(&my_mutex);
+ float yawdiff = fixup_angle((oculus_yaw-PI)*180./PI - yaw_cam);
+ float rotate = saturate(yawdiff/45., 1.);
+ drone.fly(0.,0.,0.,rotate);
+
+
+
if (first_time)
{
sem_post(&oculus_thread_go);
@@ -868,6 +890,10 @@ int main(int argc, const char** argv)
oculus_pitch = ringbuf_pitch_sensor_slow.get()/180.*PI;
oculus_roll = 0.1;
}
+
+ pthread_mutex_lock(&my_mutex);
+ oculus_yaw_global = oculus_yaw;
+ pthread_mutex_unlock(&my_mutex);
glBindFramebuffer(GL_FRAMEBUFFER, eyeFB);
diff --git a/lib.cpp b/lib.cpp
index 2b4ddd4..baed909 100644
--- a/lib.cpp
+++ b/lib.cpp
@@ -96,3 +96,17 @@ void DroneConnection::get(Mat& frame, navdata_t* nav)
frame = Mat(720,1280,CV_8UC3, buffer);
}
+
+void DroneConnection::fly(float x, float y, float z, float rot)
+{
+ char buf[100];
+ int len = snprintf(buf, sizeof(buf), "fly %f %f %f %f\n", x,y,z,rot);
+ if (len >= sizeof(buf)-1)
+ {
+ printf("ERROR: buffer too small in DroneConnection::fly()!\n");
+ return;
+ }
+ printf("%s\n",buf);
+
+ write(sockfd, buf, len);
+}
diff --git a/lib.h b/lib.h
index 3f078be..224e7f9 100644
--- a/lib.h
+++ b/lib.h
@@ -48,6 +48,7 @@ class DroneConnection
DroneConnection(const char* sockpath);
~DroneConnection();
void get(cv::Mat& frame, navdata_t* navdata);
+ void fly(float x, float y, float z, float rot);
private:
unsigned char* buffer;
diff --git a/server.py b/server.py
index 0adf238..8f2ba3a 100644
--- a/server.py
+++ b/server.py
@@ -61,6 +61,13 @@ from math import sin,cos,tan
OVERRIDE_THRESHOLD=0.01
+global_cmd_x = 0
+global_cmd_y = 0
+global_cmd_z = 0
+global_cmd_rot = 0
+global_cmd_hover = False # TODO XXX
+
+
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)
@@ -72,16 +79,24 @@ def encode_int(i):
class ServerThread(threading.Thread):
def run(self):
+
+ global global_cmd_x
+ global global_cmd_y
+ global global_cmd_z
+ global global_cmd_rot
+ global global_cmd_hover
+
while True:
# Wait for a connection
print >>sys.stderr, 'waiting for a connection'
connection, client_address = sock.accept()
+ conn2=connection.makefile()
#try:
if True:
print >>sys.stderr, 'connection from', client_address
while True:
- data = connection.recv(16)
+ data = conn2.readline()
if data:
if data=="get\n":
lock.acquire()
@@ -99,6 +114,8 @@ class ServerThread(threading.Thread):
global_cmd_hover = False # TODO XXX
lock.release()
print >>sys.stderr, "fly x/y/z/r/hov=",global_cmd_x,",",global_cmd_y,",","global_cmd_z",",",global_cmd_rot,",",global_cmd_hover
+ else:
+ print >>sys.stderr, "got corrupted command: '"+data+"'"
else:
print >>sys.stderr, 'no more data from', client_address
break
@@ -145,12 +162,6 @@ manual_override_xy = True
manual_override_z = True
manual_override_rot = True
-global_cmd_x = 0
-global_cmd_y = 0
-global_cmd_z = 0
-global_cmd_rot = 0
-global_cmd_hover = False # TODO XXX
-
drone = libardrone.ARDrone(True, True)
drone.reset()
@@ -231,6 +242,7 @@ while True:
actual_rot = js_rot
else:
actual_rot = global_cmd_rot
+ print "global_cmd_rot used, was",global_cmd_rot
drone.move_freely( not actual_hover , actual_x, actual_y, actual_z, actual_rot)
diff --git a/server2.py b/server2.py
index 587b5e5..19564cc 100644
--- a/server2.py
+++ b/server2.py
@@ -62,13 +62,14 @@ while True:
# Wait for a connection
print >>sys.stderr, 'waiting for a connection'
connection, client_address = sock.accept()
+ conn2 = connection.makefile()
try:
print >>sys.stderr, 'connection from', client_address
cap = cv2.VideoCapture("flight.avi")
logfile = open("flight.log", "r")
while True:
- data = connection.recv(16)
+ data = conn2.readline()
if data:
if data=="get\n":
status, frame = cap.read()
@@ -86,6 +87,8 @@ while True:
elif data[0:3] == "fly" and data[-1]=="\n":
values = data[3:-1].split()
print "fly ",values
+ else:
+ print "corrupted command: '"+data+"'"
else:
print >>sys.stderr, 'no more data from', client_address
break