server/client fixes, manual horizon correction, delay sensor data

4 files changed, 48 insertions, 20 deletions

diff --git a/Makefile b/Makefile
index 4e31c7d..e234fa9 100644
--- a/Makefile
+++ b/Makefile
@@ -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;
 
 
 
diff --git a/ringbuf.h b/ringbuf.h
index 1f97a12..59d43b7 100644
--- a/ringbuf.h
+++ b/ringbuf.h
@@ -33,6 +33,11 @@ class Ringbuffer
 
 		return avg;
 	}
+	
+	double front()
+	{
+		return buf[idx];
+	}
 
 	void put(double val)
 	{
diff --git a/server.py b/server.py
index ed90c74..e633579 100644
--- a/server.py
+++ b/server.py
@@ -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()