diff options
| -rw-r--r-- | client2.cpp | 28 | ||||
| -rw-r--r-- | lib.cpp | 14 | ||||
| -rw-r--r-- | lib.h | 1 | ||||
| -rw-r--r-- | server.py | 26 | ||||
| -rw-r--r-- | server2.py | 5 |
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); @@ -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); +} @@ -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; @@ -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) @@ -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 |