Diff: Threads_and_main/Read_Xtern_Sensors.cpp

Revision:

3:bc24fee36ba3

Parent:

2:e7874762cc25

--- 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];