diff options
| -rw-r--r-- | client2.cpp | 87 |
1 files changed, 44 insertions, 43 deletions
diff --git a/client2.cpp b/client2.cpp index ba3981b..999d5fe 100644 --- a/client2.cpp +++ b/client2.cpp @@ -515,20 +515,20 @@ int main(int argc, const char** argv) float px_per_deg = diag / 92.; - float total_x = 100/px_per_deg, total_y = 0; - float total_rot = 0.0; + float yaw_cam = 100/px_per_deg, pitch_cam = 0; + float roll_cam = 0.0; Mat frame(Size(1280,720), CV_8UC3), frame_(Size(1280,720), CV_8UC3), gray, oldgray; #define RINGBUF_SIZE 4 - ModuloRingbuffer ringbuf_x(RINGBUF_SIZE, -180, 180); - Ringbuffer ringbuf_y(RINGBUF_SIZE); - ModuloRingbuffer ringbuf_a(RINGBUF_SIZE, -180,180); - ModuloRingbuffer ringbuf_phi(RINGBUF_SIZE, -180,180); - ModuloRingbuffer ringbuf_psi(RINGBUF_SIZE, -180,180); - ModuloRingbuffer ringbuf_psi2(40, -180,180); - ModuloRingbuffer ringbuf_theta2(40, -180,180); - ModuloRingbuffer ringbuf_theta(RINGBUF_SIZE, -180,180); + ModuloRingbuffer ringbuf_yaw_cam(RINGBUF_SIZE, -180, 180); + Ringbuffer ringbuf_pitch_cam(RINGBUF_SIZE); + ModuloRingbuffer ringbuf_roll_cam(RINGBUF_SIZE, -180,180); + ModuloRingbuffer ringbuf_roll_sensor(RINGBUF_SIZE, -180,180); + ModuloRingbuffer ringbuf_yaw_sensor(RINGBUF_SIZE, -180,180); + ModuloRingbuffer ringbuf_yaw_sensor_slow(40, -180,180); + ModuloRingbuffer ringbuf_pitch_sensor_slow(40, -180,180); + ModuloRingbuffer ringbuf_pitch_sensor(RINGBUF_SIZE, -180,180); #define DELAY_SIZE 6 @@ -602,40 +602,41 @@ int main(int argc, const char** argv) printf("no mat!\n"); } - total_x += ( cos(total_rot*PI/180.)*shift_x + sin(total_rot*PI/180.)*shift_y) / px_per_deg; - total_y += (-sin(total_rot*PI/180.)*shift_x + cos(total_rot*PI/180.)*shift_y) / px_per_deg; - total_rot = fixup_angle(total_rot+angle); - - ringbuf_x.put(total_x); - ringbuf_y.put(total_y); //TODO - ringbuf_a.put(total_rot); - ringbuf_phi.put(navdata.phi); - ringbuf_psi.put(navdata.psi); - ringbuf_psi2.put(navdata.psi); - ringbuf_theta2.put(navdata.theta); - ringbuf_theta.put(navdata.theta); - - double xdiff = fixup_range( ringbuf_x.get() - ringbuf_psi.get(), -180, 180); - double ydiff = ringbuf_y.get() + ringbuf_theta.get(); - double adiff = fixup_angle(ringbuf_a.get() - (-ringbuf_phi.get())); + // TODO fixme proper rotation + yaw_cam += ( cos(roll_cam*PI/180.)*shift_x + sin(roll_cam*PI/180.)*shift_y) / px_per_deg; + pitch_cam += (-sin(roll_cam*PI/180.)*shift_x + cos(roll_cam*PI/180.)*shift_y) / px_per_deg; + roll_cam = fixup_angle(roll_cam+angle); + + ringbuf_yaw_cam.put(yaw_cam); + ringbuf_pitch_cam.put(pitch_cam); + ringbuf_roll_cam.put(roll_cam); + ringbuf_roll_sensor.put(navdata.phi); + ringbuf_yaw_sensor.put(navdata.psi); + ringbuf_yaw_sensor_slow.put(navdata.psi); + ringbuf_pitch_sensor_slow.put(navdata.theta); + ringbuf_pitch_sensor.put(navdata.theta); + + double yaw_diff = fixup_range( ringbuf_yaw_cam.get() - ringbuf_yaw_sensor.get(), -180, 180); + double pitch_diff = ringbuf_pitch_cam.get() + ringbuf_pitch_sensor.get(); + double roll_diff = fixup_angle(ringbuf_roll_cam.get() - (-ringbuf_roll_sensor.get())); //if (fabs(xdiff) < px_per_deg) xdiff = 0.0; //if (fabs(ydiff) < px_per_deg) ydiff = 0.0; //if (fabs(adiff) < 2) adiff = 0.0; - xdiff*=0.02; - ydiff*=0.02; - adiff*=0.5; - total_x = fixup_range(total_x - xdiff, -180, 180); - total_y = total_y - ydiff; - total_rot = fixup_angle(total_rot - adiff); - ringbuf_x.add(-xdiff); - ringbuf_y.add(-ydiff); - ringbuf_a.add(-adiff); + yaw_diff*=0.02; + pitch_diff*=0.02; + roll_diff*=0.5; + yaw_cam = fixup_range(yaw_cam - yaw_diff, -180, 180); + pitch_cam = pitch_cam - pitch_diff; + roll_cam = fixup_angle(roll_cam - roll_diff); + ringbuf_yaw_cam.add(-yaw_diff); + ringbuf_pitch_cam.add(-pitch_diff); + ringbuf_roll_cam.add(-roll_diff); - //total_x = navdata.psi; - //total_y = - navdata.theta * px_per_deg; - //total_rot = -navdata.phi; + //yaw_cam = navdata.psi; + //pitch_cam = - navdata.theta * px_per_deg; + //roll_cam = -navdata.phi; @@ -680,9 +681,9 @@ int main(int argc, const char** argv) glUseProgram(drawOnCanvasProgram); glActiveTexture(GL_TEXTURE0); glBindTexture(GL_TEXTURE_2D, texVideo); - glUniform1f(uniCamYaw,(float)total_x/180.*PI); - glUniform1f(uniCamPitch,-(float)total_y/180.*PI); - glUniform1f(uniCamRoll,-total_rot/180.*PI); + glUniform1f(uniCamYaw,(float)yaw_cam/180.*PI); + glUniform1f(uniCamPitch,-(float)pitch_cam/180.*PI); + glUniform1f(uniCamRoll,-roll_cam/180.*PI); glDrawArrays(GL_TRIANGLES, 0, 6); @@ -692,8 +693,8 @@ int main(int argc, const char** argv) glUseProgram(drawFromCanvasProgram); glActiveTexture(GL_TEXTURE0); glBindTexture(GL_TEXTURE_2D, canvasTex); - /*glUniform1f(uniEyeYaw,3.1415+ringbuf_psi2.get()/180.*PI); - glUniform1f(uniEyePitch,-ringbuf_theta2.get()/180.*PI); + /*glUniform1f(uniEyeYaw,3.1415+ringbuf_yaw_sensor_slow.get()/180.*PI); + glUniform1f(uniEyePitch,-ringbuf_pitch_sensor_slow.get()/180.*PI); glUniform1f(uniEyeRoll,0);*/ glUniform1f(uniEyeYaw,oculus_yaw); glUniform1f(uniEyePitch,-oculus_pitch); |