diff options
| -rw-r--r-- | client2.cpp | 44 | ||||
| -rw-r--r-- | 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); @@ -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"): |