From 65a270739ee8e09a33670c2c3f8af7db4bd3b3b2 Mon Sep 17 00:00:00 2001
From: Florian Jung <flo@windfisch.org>
Date: Fri, 24 Apr 2015 17:29:51 +0200
Subject: account for command lag plus testing. works quite well.

---
 client2.cpp | 44 ++++++++++++++++++++++++++++++++++++++++----
 server.py   | 29 +++++++++++++++++++++++------
 2 files changed, 63 insertions(+), 10 deletions(-)

diff --git a/client2.cpp b/client2.cpp
index 6f8e79d..7049725 100644
--- a/client2.cpp
+++ b/client2.cpp
@@ -495,6 +495,28 @@ int init_ohmd(ohmd_context** ctx_, ohmd_device** hmd_)
 	return 0;
 }
 
+int getDroneFPS()
+{
+	static time_t last = 0;
+	static int nbFrames = 0;
+	static int fps=0;
+
+     // Measure speed
+     	time_t now = time(NULL);
+
+
+     nbFrames++;
+     if ( now - last > 0 ){ // more than 1 sec ago
+	fps = (nbFrames);
+         nbFrames = 0;
+         last = now;
+
+     if (fps<=0) fps = 1;
+     }
+
+	return fps;
+}
+
 void showFPS()
 {
 	static double lastTime = 0;
@@ -550,6 +572,7 @@ 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;
+bool no_oculus;
 
 void* video_fetcher_thread(void* ptr)
 {
@@ -567,6 +590,15 @@ 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
 	
+	#define CMDLAG_SIZE 8
+	// 5: still overshoots but much more stable
+	// 7: overshoots a tiny bit
+	// 8: cool!
+	// 9: pretty accurate, oscillates a tiny bit
+	// 10: slows down before hitting the goal, oscillates a tiny bit
+	Ringbuffer pending_commands(CMDLAG_SIZE); // holds all commands that will be executed
+	                                          // by the drone, but have not been sensed
+	                                          // yet (because of laggy sensors)
 	int adjust_phi=0;//10;
 	
 	
@@ -699,10 +731,14 @@ void* video_fetcher_thread(void* ptr)
 		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.);
+		float target_yaw = no_oculus ? 0. : (oculus_yaw-PI);
+		int droneFPS = getDroneFPS();
+		printf("drone fps is %i\n",droneFPS);
+		float extrapolated_yaw_cam = yaw_cam*PI/180. + pending_commands.sum()*(1.0/droneFPS);
+		float yawdiff = fixup_angle(target_yaw - extrapolated_yaw_cam);
+		float rotate = saturate(yawdiff*5., 10.);
 		drone.fly(0.,0.,0.,rotate);
-		
+		pending_commands.put(rotate);
 
 
 		if (first_time)
@@ -720,7 +756,7 @@ void* video_fetcher_thread(void* ptr)
 
 int main(int argc, const char** argv)
 {
-	bool no_oculus = (argc>=2 && (strcmp(argv[1],"--no-oculus")==0));
+	no_oculus = (argc>=2 && (strcmp(argv[1],"--no-oculus")==0));
 
 	pthread_mutex_init(&my_mutex, NULL);
 	sem_init(&oculus_thread_go, 0, 0);
diff --git a/server.py b/server.py
index 08d26a8..ebe8308 100644
--- a/server.py
+++ b/server.py
@@ -181,6 +181,14 @@ global_batt = 0.
 global_frame =  None
 
 serverthread.start()
+
+drone.set_max_vz(750.0000)
+drone.set_max_rotspeed(1.0)
+drone.set_max_angle(0.1)
+limit_vz=750.0
+limit_rot=1.0
+limit_tilt=0.1
+
 while True:
     if no_flight == False:
         pygame.event.pump()
@@ -234,18 +242,21 @@ while True:
         if manual_override_xy:
             actual_hover, actual_x, actual_y = js_hover, js_x, js_y
         else:
-            actual_hover, actual_x, actual_y = global_cmd_hover, global_cmd_x, global_cmd_y
+            actual_hover, actual_x, actual_y = global_cmd_hover, global_cmd_x/limit_tilt, global_cmd_y/limit_tilt
+            actual_x=max(-1.0, min(1.0, actual_x))
+            actual_y=max(-1.0, min(1.0, actual_y))
 
         if manual_override_z:
             actual_z = js_z
         else:
-            actual_z = global_cmd_z
+            actual_z = global_cmd_z / limit_vz
+            actual_z=max(-1.0, min(1.0, actual_z))
 
         if manual_override_rot:
             actual_rot = js_rot
         else:
-            actual_rot = global_cmd_rot
-            print "global_cmd_rot used, was",global_cmd_rot
+            actual_rot = global_cmd_rot / limit_rot
+            actual_rot=max(-1.0, min(1.0, actual_rot))
 
         drone.move_freely( not actual_hover  , actual_x, actual_y, actual_z, actual_rot) 
     
@@ -283,13 +294,19 @@ while True:
 
     if key == ord("z"):
         drone.set_max_vz(750.0000)
-        drone.set_max_rotspeed(1.0)
-        drone.set_max_angle(0.1)
+        drone.set_max_rotspeed(10)
+        drone.set_max_angle(0.2)
+        limit_vz=750.0
+        limit_rot=10.
+        limit_tilt=0.2
         print "slow"
     elif key == ord("a"):
         drone.set_max_vz(10000.0000)
         drone.set_max_rotspeed(10)
         drone.set_max_angle(0.6)
+        limit_vz=10000.0
+        limit_rot=10.
+        limit_tilt=0.6
         print "fast"
     
     if key == ord("1"):
-- 
cgit v1.2.1