summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorFlorian Jung <flo@windfisch.org>2015-04-24 17:29:51 +0200
committerFlorian Jung <flo@windfisch.org>2015-04-24 17:34:30 +0200
commit65a270739ee8e09a33670c2c3f8af7db4bd3b3b2 (patch)
treea94b2aa1992596979e987d738082c893f43a5f52
parent7acbcfa78372e26cf2cd80cc266949434ff8db84 (diff)
account for command lag plus testing. works quite well.
-rw-r--r--client2.cpp44
-rw-r--r--server.py29
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"):