diff options
| -rw-r--r-- | client2.cpp | 11 |
1 files changed, 7 insertions, 4 deletions
diff --git a/client2.cpp b/client2.cpp index 6f8e79d..e2f53b7 100644 --- a/client2.cpp +++ b/client2.cpp @@ -46,6 +46,7 @@ #include <opencv2/opencv.hpp> #include "lib.h" #include "ringbuf.h" +#include "regulator.h" #define PI 3.141593654 @@ -136,7 +137,7 @@ const char* drawFromCanvasFragmentSource = "uniform float eye_pitch;\n" "uniform float eye_roll;\n" "const float aspect_ratio=1280./2/720.;\n" - "const float horiz_field_of_view=90/180.*3.141592654;\n" + "const float horiz_field_of_view=60/180.*3.141592654;\n" "const float CAM_FX=1/2.0 / tan(horiz_field_of_view/2.0);\n" "const mat3 eye_cal_inv = transpose(mat3(1/CAM_FX, 0, -1/2/CAM_FX, 0, 1/CAM_FX, -1/aspect_ratio/2/CAM_FX, 0,0,1));\n" "const mat3 opencv_to_math = mat3(0,1,0, 0,0,1, -1,0,0);\n" @@ -568,7 +569,8 @@ void* video_fetcher_thread(void* ptr) Ringbuffer delay_theta(DELAY_SIZE); // the sensor data int adjust_phi=0;//10; - + int time_now=0; // in msec + Regulator regulator(1./45.,0,0, 0); DroneConnection drone(SOCKETPATH); navdata_t navdata; @@ -700,8 +702,8 @@ void* video_fetcher_thread(void* ptr) 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); + regulator.put(yawdiff, time_now); + drone.fly(0.,0.,0.,saturate(regulator.get(),1.0)); @@ -714,6 +716,7 @@ void* video_fetcher_thread(void* ptr) oldgray = gray.clone(); usleep(40000); + time_now += 40; } return NULL; } |