Diff: rplidar/RPlidar.cpp

Revision:

5:fe76f3dae81e

Parent:

4:de5a65c17664

Child:

7:0dac9d4ff04f

--- a/rplidar/RPlidar.cpp	Sun Jun 05 09:43:40 2016 +0000
+++ b/rplidar/RPlidar.cpp	Tue Jun 07 03:26:08 2016 +0000
@@ -28,7 +28,7 @@
  */
 
 #include "rplidar.h"
-BufferedSerial _bined_serialdev( PA_11,  PA_12); // tx, rx
+//BufferedSerial _bined_serialdev( PA_11,  PA_12); // tx, rx
 Timer timers;
 RPLidar::RPLidar()
 {
@@ -60,12 +60,11 @@
     _bined_serialdev->baud(RPLIDAR_SERIAL_BAUDRATE);
 }
 */
-void RPLidar::begin()
+void RPLidar::begin(BufferedSerial &serialobj)
 {
-//BufferedSerial lidar_serial(PinName PA_11, PinName PA_12);
-    //Serial.begin(115200);
+    _bined_serialdev = &serialobj;
     timers.start();
-    _bined_serialdev.baud(115200);
+    _bined_serialdev->baud(RPLIDAR_SERIAL_BAUDRATE);
 }
 // close the currently opened serial interface
 void RPLidar::end()
@@ -119,7 +118,7 @@
         }
 
         while ((remainingtime=timers.read_ms() - currentTs) <= timeout) {
-            int currentbyte = _bined_serialdev.getc();
+            int currentbyte = _bined_serialdev->getc();
             if (currentbyte < 0) continue;
 
             infobuf[recvPos++] = currentbyte;
@@ -162,7 +161,7 @@
         }
 
         while ((remainingtime=timers.read_ms() - currentTs) <= timeout) {
-            int currentbyte = _bined_serialdev.getc();
+            int currentbyte = _bined_serialdev->getc();
             if (currentbyte<0) continue;
             infobuf[recvPos++] = currentbyte;
 
@@ -186,17 +185,6 @@
 // start the measurement operation
 u_result RPLidar::startScan(bool force, _u32 timeout)
 {
-  /*
-  rplidar_cmd_packet_t pkt_header;
-  rplidar_cmd_packet_t * header = &pkt_header;
-  header->syncByte = 0xA5;
-  header->cmd_flag = 0x21;
-  //se.write(12);
-
-  _bined_serialdev.putc(header->syncByte );
-  _bined_serialdev.putc(header->cmd_flag );
-  */
-
     u_result ans;
 
 //    if (!isOpen()) return RESULT_OPERATION_FAIL;
@@ -236,7 +224,7 @@
    _u8 recvPos = 0;
 
    while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
-        int currentbyte = _bined_serialdev.getc();
+        int currentbyte = _bined_serialdev->getc();
         if (currentbyte<0) continue;
 //Serial.println(currentbyte);
         switch (recvPos) {
@@ -269,7 +257,7 @@
               _currentMeasurement.angle = (node.angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
               _currentMeasurement.quality = (node.sync_quality>>RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
               _currentMeasurement.startBit = (node.sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT);
-                ang = _currentMeasurement.angle;
+              ang = _currentMeasurement.angle;
               dis = _currentMeasurement.distance;
 
 
@@ -289,6 +277,9 @@
   angMin = min;
   angMax = max;
 }
+void RPLidar::get_lidar(){
+  if (!IS_OK(waitPoint())) startScan();
+}
 u_result RPLidar::_sendCommand(_u8 cmd, const void * payload, size_t payloadsize)
 {
 
@@ -304,8 +295,8 @@
     header->cmd_flag = cmd;
 
     // send header first
-    _bined_serialdev.putc(header->syncByte );
-    _bined_serialdev.putc(header->cmd_flag );
+    _bined_serialdev->putc(header->syncByte );
+    _bined_serialdev->putc(header->cmd_flag );
 
   //  _bined_serialdev->write( (uint8_t *)header, 2);
 
@@ -321,7 +312,7 @@
 
         // send size
         _u8 sizebyte = payloadsize;
-        _bined_serialdev.putc(sizebyte);
+        _bined_serialdev->putc(sizebyte);
       //  _bined_serialdev->write((uint8_t *)&sizebyte, 1);
 
         // send payload
@@ -329,7 +320,7 @@
     //    _bined_serialdev->write((uint8_t *)&payload, sizebyte);
 
         // send checksum
-        _bined_serialdev.putc(checksum);
+        _bined_serialdev->putc(checksum);
       //  _bined_serialdev->write((uint8_t *)&checksum, 1);
 
     }
@@ -345,7 +336,7 @@
     _u8 *headerbuf = (_u8*)header;
     while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
 
-        int currentbyte = _bined_serialdev.getc();
+        int currentbyte = _bined_serialdev->getc();
         if (currentbyte<0) continue;
         switch (recvPos) {
         case 0: