branch for tests with T265
Dependencies: Lib_Cntrl AHRS Lib_Misc
Diff: Threads_and_main/Read_Xtern_Sensors.cpp
- Revision:
- 3:bc24fee36ba3
- Parent:
- 2:e7874762cc25
diff -r e7874762cc25 -r bc24fee36ba3 Threads_and_main/Read_Xtern_Sensors.cpp --- a/Threads_and_main/Read_Xtern_Sensors.cpp Mon Oct 21 17:16:11 2019 +0000 +++ b/Threads_and_main/Read_Xtern_Sensors.cpp Mon Oct 28 07:54:10 2019 +0000 @@ -16,10 +16,12 @@ if(optical_flow.update_integral()) { data.sens_OF[0] = optical_flow.avg_flowx()*0.001f; data.sens_OF[1] = optical_flow.avg_flowy()*0.001f; - data.sens_OF[2] = 0.0f; // later: Ultrasonic distance from OF - data.sens_OF[3] = (float)optical_flow.avg_scaled_quality(); - data.sens_OF[4] = optical_flow.avg_gyro_x(); - data.sens_OF[5] = optical_flow.avg_gyro_y(); + data.sens_OF[2] = (float)optical_flow.avg_scaled_quality(); + data.sens_OF[3] = optical_flow.avg_gyro_x(); + data.sens_OF[4] = optical_flow.avg_gyro_y(); + data.sens_OF[5] = optical_flow.avg_gyro_z(); + data.est_Vxyz[0]= (-data.sens_OF[0] + data.sens_gyr[1] * data.sens_Lidar[2]); + data.est_Vxyz[1]= ( data.sens_OF[1] - data.sens_gyr[0] * data.sens_Lidar[2]); } data.sens_Lidar[0] = tfmini_1(); data.est_xyz[2] = data.sens_Lidar[0];