diff options
| -rw-r--r-- | Makefile | 2 | ||||
| -rw-r--r-- | client2.cpp | 50 | ||||
| -rw-r--r-- | ringbuf.h | 5 | ||||
| -rw-r--r-- | server.py | 11 |
4 files changed, 48 insertions, 20 deletions
@@ -1,4 +1,4 @@ -client2: client2.cpp lib.cpp +client2: client2.cpp lib.cpp ringbuf.h g++ -std=c++11 -g client2.cpp lib.cpp -lglfw -lGLEW -lGLU -lGL `pkg-config --libs opencv` -lm -o client2 client: client.c gcc client.c -lX11 -lXi -lXmu -lglut -lGL -lGLU -lm -o client diff --git a/client2.cpp b/client2.cpp index 49eb776..e412878 100644 --- a/client2.cpp +++ b/client2.cpp @@ -81,13 +81,13 @@ float vertices[] = { float quadVertices[] = { - -1.f, -1.f, 0.4f,0.4f, // Vertex 1 (X, Y) - 1.f, -1.f, 0.6f,0.4f, // Vertex 2 (X, Y) - 1.f, 1.f, 0.6f,0.6f, // Vertex 3 (X, Y) + -1.f, -1.f, 0.f,0.f, // Vertex 1 (X, Y) + 1.f, -1.f, 1.f,0.f, // Vertex 2 (X, Y) + 1.f, 1.f, 1.f,1.f, // Vertex 3 (X, Y) - 1.f, 1.f, 0.6f,0.6f, // Vertex 3 (X, Y) - -1.f, 1.f, 0.4f,0.6f, // Vertex 4 (X, Y) - -1.f, -1.f, 0.4f,0.4f // Vertex 1 (X, Y) + 1.f, 1.f, 1.f,1.f, // Vertex 3 (X, Y) + -1.f, 1.f, 0.f,1.f, // Vertex 4 (X, Y) + -1.f, -1.f, 0.f,0.f // Vertex 1 (X, Y) }; @@ -317,7 +317,7 @@ int main(int argc, const char** argv) Mat screencontent_(real_canvas_height,real_canvas_width, CV_8UC3); Mat screencontent_mask(real_canvas_height,real_canvas_width, CV_8UC3); - #define RINGBUF_SIZE 10 + #define RINGBUF_SIZE 4 ModuloRingbuffer ringbuf_x(RINGBUF_SIZE, -virtual_canvas_width/2, virtual_canvas_width/2); Ringbuffer ringbuf_y(RINGBUF_SIZE); ModuloRingbuffer ringbuf_a(RINGBUF_SIZE, -180,180); @@ -326,6 +326,12 @@ int main(int argc, const char** argv) ModuloRingbuffer ringbuf_theta(RINGBUF_SIZE, -180,180); + #define DELAY_SIZE 6 + Ringbuffer delay_phi(DELAY_SIZE); // should delay sensor data by ~0.2sec + Ringbuffer delay_psi(DELAY_SIZE); // that's the amount the video lags behind + Ringbuffer delay_theta(DELAY_SIZE); // the sensor data + + for (int i=0; i<400;i++) { drone.get(frame_, &navdata); @@ -333,9 +339,22 @@ int main(int argc, const char** argv) cvtColor(frame, oldgray, COLOR_BGR2GRAY); } - while (waitKey(1) != 'x') + char key; + int adjust_phi=0; + while ((key=waitKey(1)) != 'x') { drone.get(frame_, &navdata); + delay_phi.put(navdata.phi); + delay_psi.put(navdata.psi); + delay_theta.put(navdata.theta); + navdata.phi = delay_phi.front(); + navdata.psi = delay_psi.front(); + navdata.theta = delay_theta.front(); + + navdata.phi = fixup_angle(navdata.phi + adjust_phi); + + if (key=='q') adjust_phi++; + if (key=='w') adjust_phi--; //for (int i=0; i<1280; i+=50) frame_.col(i)=Scalar(0,255,255); //for (int i=0; i<720; i+=50) frame_.row(i)=Scalar(0,255,255); @@ -367,8 +386,8 @@ int main(int argc, const char** argv) printf("no mat!\n"); } - total_x += shift_x; - total_y += shift_y; + total_x += cos(total_rot*PI/180.)*shift_x + sin(total_rot*PI/180.)*shift_y; + total_y += -sin(total_rot*PI/180.)*shift_x + cos(total_rot*PI/180.)*shift_y; total_rot = fixup_angle(total_rot+angle); ringbuf_x.put(total_x); @@ -386,16 +405,19 @@ int main(int argc, const char** argv) //if (fabs(ydiff) < px_per_deg) ydiff = 0.0; //if (fabs(adiff) < 2) adiff = 0.0; - xdiff*=0.0; - ydiff*=0.0; - adiff*=0.1; + xdiff*=0.5; + ydiff*=0.5; + adiff*=0.5; total_x = fixup_range(total_x - xdiff, -virtual_canvas_width/2, virtual_canvas_width/2); total_y = total_y - ydiff; total_rot = fixup_angle(total_rot - adiff); ringbuf_x.add(-xdiff); ringbuf_y.add(-ydiff); ringbuf_a.add(-adiff); - + + //total_x = navdata.psi * px_per_deg; + //total_y = - navdata.theta * px_per_deg; + //total_rot = -navdata.phi; @@ -33,6 +33,11 @@ class Ringbuffer return avg; } + + double front() + { + return buf[idx]; + } void put(double val) { @@ -27,7 +27,8 @@ class ServerThread(threading.Thread): # Wait for a connection print >>sys.stderr, 'waiting for a connection' connection, client_address = sock.accept() - try: + #try: + if True: print >>sys.stderr, 'connection from', client_address while True: @@ -36,7 +37,7 @@ class ServerThread(threading.Thread): if data=="get\n": lock.acquire() framestr = global_frame.tostring() - lenframestr=len(global_framestr) + lenframestr=len(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": @@ -51,9 +52,9 @@ class ServerThread(threading.Thread): else: print >>sys.stderr, 'no more data from', client_address break - except: - print "Dingens!!11!1!!!" - finally: + #except: + # print "Dingens!!11!1!!!" + #finally: # Clean up the connection connection.close() |