Diff: VL6180.cpp

Revision:

11:0b9098ec11c7

Parent:

10:c752a8d76ee2

Child:

12:82b8fe254222

--- a/VL6180.cpp	Tue Dec 17 16:33:46 2019 +0000
+++ b/VL6180.cpp	Thu Dec 19 00:13:38 2019 +0000
@@ -5,11 +5,17 @@
 #include "VL6180.hpp"
 #include <vector>
 
-#define DEBUG_TOF
+//#define DEBUG_TOF
 #ifdef DEBUG_TOF
 Serial debug_term(SERIAL_TX, SERIAL_RX);
 #endif
 
+
+#define ROS_TOF
+int Global_TOF_Ranges[num_VL6180];
+bool TOF_Range_Flag;
+Mutex Global_TOF_Range_Mutex;
+
 /*------------------------------------------------------------------------------
 //  Main Program loop
 ------------------------------------------------------------------------------*/
@@ -44,6 +50,18 @@
 
             wait_ms(10);
         }
+        
+#ifdef ROS_TOF
+    Global_TOF_Range_Mutex.lock();
+    for(int IDX = 0; IDX < num_VL6180; IDX++) {
+        Global_TOF_Ranges[IDX] = Range[IDX];
+    }
+    if(TOF_Range_Flag == false)
+    {
+        TOF_Range_Flag = true;
+    }
+    Global_TOF_Range_Mutex.unlock();
+#endif
 
 #ifdef DEBUG_TOF
         debug_term.printf("|--------------------------------\n\r"