sra-romi
Dependencies: BufferedSerial Matrix
Diff: rplidar/RPlidar.cpp
- Revision:
- 2:e27733eaa594
- Parent:
- 0:2b691d200d6f
diff -r dc87724abce8 -r e27733eaa594 rplidar/RPlidar.cpp --- 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; }