diff options
Diffstat (limited to 'client2.cpp')
| -rw-r--r-- | client2.cpp | 28 |
1 files changed, 27 insertions, 1 deletions
diff --git a/client2.cpp b/client2.cpp index 8d2e480..90372fa 100644 --- a/client2.cpp +++ b/client2.cpp @@ -528,6 +528,19 @@ float deadzone(float val, float dead) return val; } +float saturate(float val, float sat) +{ + if (val < 0.) + { + if (val < -sat) val = -sat; + } + else + { + if (val > sat) val = sat; + } + return val; +} + pthread_mutex_t my_mutex; sem_t oculus_thread_go; @@ -535,6 +548,7 @@ sem_t oculus_thread_go; ModuloRingbuffer ringbuf_yaw_sensor_slow(40, -180,180); 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; void* video_fetcher_thread(void* ptr) @@ -553,7 +567,7 @@ 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 - int adjust_phi=10; + int adjust_phi=0;//10; DroneConnection drone(SOCKETPATH); @@ -680,8 +694,16 @@ void* video_fetcher_thread(void* ptr) ringbuf_yaw_sensor_slow.put(navdata.psi); ringbuf_pitch_sensor_slow.put(navdata.theta); + + 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.); + drone.fly(0.,0.,0.,rotate); + + + if (first_time) { sem_post(&oculus_thread_go); @@ -868,6 +890,10 @@ int main(int argc, const char** argv) oculus_pitch = ringbuf_pitch_sensor_slow.get()/180.*PI; oculus_roll = 0.1; } + + pthread_mutex_lock(&my_mutex); + oculus_yaw_global = oculus_yaw; + pthread_mutex_unlock(&my_mutex); glBindFramebuffer(GL_FRAMEBUFFER, eyeFB); |