Diff: rplidar/RPlidar.cpp

Revision:

2:e27733eaa594

Parent:

0:2b691d200d6f

--- a/rplidar/RPlidar.cpp	Thu Apr 11 09:51:28 2019 +0000
+++ b/rplidar/RPlidar.cpp	Thu Apr 11 13:24:08 2019 +0000
@@ -52,10 +52,15 @@
     while(1)
     {   
     
+        mutex_measurements.lock();
         while (IS_OK(pollPoint()))
-        {              
+        {    
+            
+            currentMeasurement_fromThread = getCurrentPoint();
+            newMeasurement = 1;
+               
           
-            if(!mail_box.full())
+            /*if(!mail_box.full())
             {            
                 struct RPLidarMeasurement current = getCurrentPoint();
                 struct RPLidarMeasurement *mail = mail_box.alloc();
@@ -95,9 +100,10 @@
                 }
                 
 
-            }
+            }*/
         }
-        wait(0.02);
+        mutex_measurements.unlock();    
+        wait_us(2000);
     }   
 
     
@@ -109,7 +115,22 @@
 */
 int32_t RPLidar::pollSensorData(struct RPLidarMeasurement *_data)
 {
-    osEvent evt = mail_box.get();
+    
+    mutex_measurements.lock();
+    if(newMeasurement)
+    {
+        //currentMeasurement_fromThread = getCurrentPoint();
+        _data->distance = currentMeasurement_fromThread.distance;
+        _data->angle = currentMeasurement_fromThread.angle;
+        _data->quality = currentMeasurement_fromThread.quality;
+        _data->startBit = currentMeasurement_fromThread.startBit;
+        newMeasurement = 0;
+        mutex_measurements.unlock();    
+        return 0;
+    }
+    mutex_measurements.unlock();    
+    
+    /*osEvent evt = mail_box.get();
     if (evt.status == osEventMail) {
         
         struct RPLidarMeasurement *mail = (struct RPLidarMeasurement*)evt.value.p;
@@ -123,7 +144,7 @@
 
         
 
-    }
+    }*/
     
     return -1;
 }