Revision 0:2b691d200d6f, committed 2019-04-09

Comitter:

LuisRA

Date:

Tue Apr 09 17:53:31 2019 +0000

Child:

1:dc87724abce8

Commit message:

initial commit

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BufferedSerial.lib	Tue Apr 09 17:53:31 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/sam_grove/code/BufferedSerial/#7e5e866edd3d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/inc/rplidar_cmd.h	Tue Apr 09 17:53:31 2019 +0000
@@ -0,0 +1,99 @@
+/*
+ * Copyright (c) 2014, RoboPeak
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+/*
+ *  RoboPeak LIDAR System
+ *  Data Packet IO packet definition for RP-LIDAR
+ *
+ *  Copyright 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *
+ */
+
+
+#pragma once
+#include "rptypes.h"
+#include "rplidar_protocol.h"
+
+// Commands
+//-----------------------------------------
+
+// Commands without payload and response
+#define RPLIDAR_CMD_STOP               0x25
+#define RPLIDAR_CMD_SCAN               0x20
+#define RPLIDAR_CMD_FORCE_SCAN         0x21
+#define RPLIDAR_CMD_RESET              0x40
+
+
+// Commands without payload but have response
+#define RPLIDAR_CMD_GET_DEVICE_INFO      0x50
+#define RPLIDAR_CMD_GET_DEVICE_HEALTH    0x52
+
+#if defined(_WIN32)
+#pragma pack(1)
+#endif
+
+
+// Response
+// ------------------------------------------
+#define RPLIDAR_ANS_TYPE_MEASUREMENT      0x81
+
+#define RPLIDAR_ANS_TYPE_DEVINFO          0x4
+#define RPLIDAR_ANS_TYPE_DEVHEALTH        0x6
+
+
+#define RPLIDAR_STATUS_OK                 0x0
+#define RPLIDAR_STATUS_WARNING            0x1
+#define RPLIDAR_STATUS_ERROR              0x2
+
+#define RPLIDAR_RESP_MEASUREMENT_SYNCBIT        (0x1<<0)
+#define RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT  2
+#define RPLIDAR_RESP_MEASUREMENT_CHECKBIT       (0x1<<0)
+#define RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT    1
+
+typedef struct _rplidar_response_measurement_node_t {
+    _u8    sync_quality;      // syncbit:1;syncbit_inverse:1;quality:6;
+    _u16   angle_q6_checkbit; // check_bit:1;angle_q6:15;
+    _u16   distance_q2;
+} __attribute__((packed)) rplidar_response_measurement_node_t;
+
+typedef struct _rplidar_response_device_info_t {
+    _u8   model;
+    _u16  firmware_version;
+    _u8   hardware_version;
+    _u8   serialnum[16];
+} __attribute__((packed)) rplidar_response_device_info_t;
+
+typedef struct _rplidar_response_device_health_t {
+    _u8   status;
+    _u16  error_code;
+} __attribute__((packed)) rplidar_response_device_health_t;
+
+#if defined(_WIN32)
+#pragma pack()
+#endif
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/inc/rplidar_protocol.h	Tue Apr 09 17:53:31 2019 +0000
@@ -0,0 +1,76 @@
+/*
+ * Copyright (c) 2014, RoboPeak
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+/*
+ *  RoboPeak LIDAR System
+ *  Data Packet IO protocol definition for RP-LIDAR
+ *
+ *  Copyright 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  
+ */
+//#ifndef RPLIDAR_PROTOCOL_H
+//#define RPLIDAR_PROTOCOL_H
+
+#pragma once
+
+// RP-Lidar Input Packets
+
+#define RPLIDAR_CMD_SYNC_BYTE        0xA5
+#define RPLIDAR_CMDFLAG_HAS_PAYLOAD  0x80
+
+
+#define RPLIDAR_ANS_SYNC_BYTE1       0xA5
+#define RPLIDAR_ANS_SYNC_BYTE2       0x5A
+
+#define RPLIDAR_ANS_PKTFLAG_LOOP     0x1
+
+
+#if defined(_WIN32)
+#pragma pack(1)
+#endif
+
+typedef struct _rplidar_cmd_packet_t {
+    _u8 syncByte; //must be RPLIDAR_CMD_SYNC_BYTE
+    _u8 cmd_flag; 
+    _u8 size;
+    _u8 data[0];
+} __attribute__((packed)) rplidar_cmd_packet_t;
+
+
+typedef struct _rplidar_ans_header_t {
+    _u8  syncByte1; // must be RPLIDAR_ANS_SYNC_BYTE1
+    _u8  syncByte2; // must be RPLIDAR_ANS_SYNC_BYTE2
+    _u32 size:30;
+    _u32 subType:2;
+    _u8  type;
+} __attribute__((packed)) rplidar_ans_header_t;
+
+#if defined(_WIN32)
+#pragma pack()
+#endif
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/inc/rptypes.h	Tue Apr 09 17:53:31 2019 +0000
@@ -0,0 +1,116 @@
+/*
+ * Copyright (c) 2014, RoboPeak
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification, 
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY 
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 
+ * SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 
+ * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR 
+ * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+/*
+ *  RoboPeak LIDAR System
+ *  Common Types definition
+ *
+ *  Copyright 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ * 
+ */
+
+#pragma once
+
+
+#ifdef _WIN32
+
+//fake stdint.h for VC only
+
+typedef signed   char     int8_t;
+typedef unsigned char     uint8_t;
+
+typedef __int16           int16_t;
+typedef unsigned __int16  uint16_t;
+
+typedef __int32           int32_t;
+typedef unsigned __int32  uint32_t;
+
+typedef __int64           int64_t;
+typedef unsigned __int64  uint64_t;
+
+#else
+
+#include <stdint.h>
+
+#endif
+
+
+//based on stdint.h
+typedef int8_t         _s8;
+typedef uint8_t        _u8;
+
+typedef int16_t        _s16;
+typedef uint16_t       _u16;
+
+typedef int32_t        _s32;
+typedef uint32_t       _u32;
+
+typedef int64_t        _s64;
+typedef uint64_t       _u64;
+
+#define __small_endian
+
+#ifndef __GNUC__
+#define __attribute__(x)
+#endif
+
+
+// The _word_size_t uses actual data bus width of the current CPU
+#ifdef _AVR_
+typedef _u8            _word_size_t;
+#define THREAD_PROC    
+#elif defined (WIN64)
+typedef _u64           _word_size_t;
+#define THREAD_PROC    __stdcall
+#elif defined (WIN32)
+typedef _u32           _word_size_t;
+#define THREAD_PROC    __stdcall
+#elif defined (__GNUC__)
+typedef unsigned long  _word_size_t;
+#define THREAD_PROC   
+#elif defined (__ICCARM__)
+typedef _u32            _word_size_t;
+#define THREAD_PROC  
+#endif
+
+
+typedef uint32_t u_result;
+
+#define RESULT_OK              0
+#define RESULT_FAIL_BIT        0x80000000
+#define RESULT_ALREADY_DONE    0x20
+#define RESULT_INVALID_DATA    (0x8000 | RESULT_FAIL_BIT)
+#define RESULT_OPERATION_FAIL  (0x8001 | RESULT_FAIL_BIT)
+#define RESULT_OPERATION_TIMEOUT  (0x8002 | RESULT_FAIL_BIT)
+#define RESULT_OPERATION_STOP    (0x8003 | RESULT_FAIL_BIT)
+#define RESULT_OPERATION_NOT_SUPPORT    (0x8004 | RESULT_FAIL_BIT)
+#define RESULT_FORMAT_NOT_SUPPORT    (0x8005 | RESULT_FAIL_BIT)
+#define RESULT_INSUFFICIENT_MEMORY   (0x8006 | RESULT_FAIL_BIT)
+
+#define IS_OK(x)    ( ((x) & RESULT_FAIL_BIT) == 0 )
+#define IS_FAIL(x)  ( ((x) & RESULT_FAIL_BIT) )
+
+typedef _word_size_t (THREAD_PROC * thread_proc_t ) ( void * );

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Apr 09 17:53:31 2019 +0000
@@ -0,0 +1,39 @@
+#include "mbed.h"
+#include "BufferedSerial.h"
+#include "rplidar.h"
+
+Serial pc(SERIAL_TX, SERIAL_RX, 1000000);
+RPLidar lidar;
+BufferedSerial se_lidar(PA_9, PA_10);
+PwmOut rplidar_motor(A3);
+
+
+
+int main()
+{
+
+    pc.baud(1000000);//115200);
+    pc.printf("main\n");
+    wait(1);
+
+    rplidar_motor.period(0.001f);
+    lidar.begin(se_lidar);
+    lidar.setAngle(0,360);
+
+    
+    pc.printf("Program started.\n");
+    
+    
+    lidar.startThreadScan();
+    
+
+    while(1) {
+        struct RPLidarMeasurement data;
+        // poll for measurements. Returns -1 if no new measurements are available. returns 0 if found one.
+        if(lidar.pollSensorData(&data) == 0)
+        {
+            pc.printf("%f\t%f\t%d\t%c\n", data.distance, data.angle, data.quality, data.startBit);
+        }
+       wait(0.1); 
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-os.lib	Tue Apr 09 17:53:31 2019 +0000
@@ -0,0 +1,1 @@
+https://github.com/armmbed/mbed-os/#51d55508e8400b60af467005646c4e2164738d48

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rplidar/RPlidar.cpp	Tue Apr 09 17:53:31 2019 +0000
@@ -0,0 +1,570 @@
+/*
+ * RoboPeak RPLIDAR Driver for Arduino
+ * RoboPeak.com
+ *
+ * Copyright (c) 2014, RoboPeak
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
+ * SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+ * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+ * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "rplidar.h"
+//BufferedSerial _bined_serialdev( PA_11,  PA_12); // tx, rx
+Timer timers;
+
+/*
+    Thread that handles reading the sensor measurements
+    
+    This thread parses the buffered serial bytes into Lidar measurements.
+    If there's space in the mail box it adds a measurement to it. In case there 
+    isn't space, it takes 1 element out of the mail box and then puts the new 
+    measurement. This makes sure recent data is in the mailbox instead of really 
+    old data. Messages are lost anyway but this way the application will read 
+    measurements of the current robot position.
+    if the mail box still has 100 max messages then this means that the oldest
+    measurement was 50ms in a full mail box. 
+    Note that since the thread runs every 20ms then there will be measurements 
+    as old as 20ms.
+*/
+void RPLidar::Thread_readSensor(void)
+{
+
+    //pc->printf("starting thread\n");
+    while(1)
+    {   
+    
+        while (IS_OK(pollPoint()))
+        {              
+          
+            if(!mail_box.full())
+            {            
+                struct RPLidarMeasurement current = getCurrentPoint();
+                struct RPLidarMeasurement *mail = mail_box.alloc();
+                if(mail == NULL)
+                {   
+                    //there was an error allocating space in heap
+                    continue;
+                }
+                mail->distance = current.distance;
+                mail->angle = current.angle;
+                mail->quality = current.quality;
+                mail->startBit = current.startBit;
+                
+                mail_box.put(mail);
+                
+            }
+            else
+            {
+                osEvent evt = mail_box.get();
+                if (evt.status == osEventMail) {
+                    struct RPLidarMeasurement *mail = (struct RPLidarMeasurement*)evt.value.p;
+                    mail_box.free(mail);
+                    
+                    struct RPLidarMeasurement current = getCurrentPoint();
+                    mail = mail_box.alloc();
+                    if(mail == NULL)
+                    {   
+                        //there was an error allocating space in heap
+                        continue;
+                    }
+                    mail->distance = current.distance;
+                    mail->angle = current.angle;
+                    mail->quality = current.quality;
+                    mail->startBit = current.startBit;
+                    
+                    mail_box.put(mail);
+                }
+                
+
+            }
+        }
+        wait(0.02);
+    }   
+
+    
+}
+
+/*
+  Poll for new measurements in the mail box
+  Returns: 0 if found data, -1 if not data available.
+*/
+int32_t RPLidar::pollSensorData(struct RPLidarMeasurement *_data)
+{
+    osEvent evt = mail_box.get();
+    if (evt.status == osEventMail) {
+        
+        struct RPLidarMeasurement *mail = (struct RPLidarMeasurement*)evt.value.p;
+        //struct RPLidarMeasurement data;
+        _data->distance = mail->distance;
+        _data->angle = mail->angle;
+        _data->quality = mail->quality;
+        _data->startBit = mail->startBit;
+        mail_box.free(mail);
+        return 0;
+
+        
+
+    }
+    
+    return -1;
+}
+
+
+RPLidar::RPLidar()
+{
+    
+
+    _currentMeasurement.distance = 0;
+    _currentMeasurement.angle = 0;
+    _currentMeasurement.quality = 0;
+    _currentMeasurement.startBit = 0;
+    
+    useThread = 0;
+    thread.set_priority(osPriorityAboveNormal);
+    //Thread thread(osPriorityAboveNormal, OS_STACK_SIZE, NULL, "RPLidar");
+    //thread_p = &thread;
+}
+
+
+RPLidar::~RPLidar()
+{
+    end();
+}
+
+// open the given serial interface and try to connect to the RPLIDAR
+/*
+bool RPLidar::begin(BufferedSerial &serialobj)
+{
+
+    //Serial.begin(115200);
+
+    if (isOpen()) {
+      end();
+    }
+    _bined_serialdev = &serialobj;
+  //  _bined_serialdev->end();
+    _bined_serialdev->baud(RPLIDAR_SERIAL_BAUDRATE);
+}
+*/
+
+void RPLidar::begin(BufferedSerial &serialobj)
+{
+    _bined_serialdev = &serialobj;
+    timers.start();
+    _bined_serialdev->baud(RPLIDAR_SERIAL_BAUDRATE);
+    recvPos_g = 0;
+}
+// close the currently opened serial interface
+void RPLidar::end()
+{/*
+    if (isOpen()) {
+       _bined_serialdev->end();
+       _bined_serialdev = NULL;
+    }*/
+}
+
+
+// check whether the serial interface is opened
+/*
+bool RPLidar::isOpen()
+{
+    return _bined_serialdev?true:false;
+}
+*/
+// ask the RPLIDAR for its health info
+
+u_result RPLidar::getHealth(rplidar_response_device_health_t & healthinfo, _u32 timeout)
+{
+    _u32 currentTs = timers.read_ms();
+    _u32 remainingtime;
+
+    _u8 *infobuf = (_u8 *)&healthinfo;
+    _u8 recvPos = 0;
+
+    rplidar_ans_header_t response_header;
+    u_result  ans;
+
+
+  //  if (!isOpen()) return RESULT_OPERATION_FAIL;
+
+    {
+        if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_HEALTH, NULL, 0))) {
+            return ans;
+        }
+
+        if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
+            return ans;
+        }
+
+        // verify whether we got a correct header
+        if (response_header.type != RPLIDAR_ANS_TYPE_DEVHEALTH) {
+            return RESULT_INVALID_DATA;
+        }
+
+        if ((response_header.size) < sizeof(rplidar_response_device_health_t)) {
+            return RESULT_INVALID_DATA;
+        }
+
+        while ((remainingtime=timers.read_ms() - currentTs) <= timeout) {
+            int currentbyte = _bined_serialdev->getc();
+            if (currentbyte < 0) continue;
+
+            infobuf[recvPos++] = currentbyte;
+
+            if (recvPos == sizeof(rplidar_response_device_health_t)) {
+                return RESULT_OK;
+            }
+        }
+    }
+    return RESULT_OPERATION_TIMEOUT;
+}
+// ask the RPLIDAR for its device info like the serial number
+u_result RPLidar::getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout )
+{
+    _u8  recvPos = 0;
+    _u32 currentTs = timers.read_ms();
+    _u32 remainingtime;
+    _u8 *infobuf = (_u8*)&info;
+    rplidar_ans_header_t response_header;
+    u_result  ans;
+
+  //  if (!isOpen()) return RESULT_OPERATION_FAIL;
+
+    {
+        if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_INFO,NULL,0))) {
+            return ans;
+        }
+
+        if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
+            return ans;
+        }
+
+        // verify whether we got a correct header
+        if (response_header.type != RPLIDAR_ANS_TYPE_DEVINFO) {
+            return RESULT_INVALID_DATA;
+        }
+
+        if (response_header.size < sizeof(rplidar_response_device_info_t)) {
+            return RESULT_INVALID_DATA;
+        }
+
+        while ((remainingtime=timers.read_ms() - currentTs) <= timeout) {
+            int currentbyte = _bined_serialdev->getc();
+            if (currentbyte<0) continue;
+            infobuf[recvPos++] = currentbyte;
+
+            if (recvPos == sizeof(rplidar_response_device_info_t)) {
+                return RESULT_OK;
+            }
+        }
+    }
+
+    return RESULT_OPERATION_TIMEOUT;
+}
+
+// stop the measurement operation
+u_result RPLidar::stop()
+{
+//    if (!isOpen()) return RESULT_OPERATION_FAIL;
+    u_result ans = _sendCommand(RPLIDAR_CMD_STOP,NULL,0);
+    return ans;
+}
+
+
+
+u_result RPLidar::startThreadScan()
+{
+    startScan();
+    useThread = true;
+    thread.start(callback(this, &RPLidar::Thread_readSensor));
+        
+    return RESULT_OK;
+}
+// start the measurement operation
+/*
+ This was altered to also start the thread that parses measurements in the background
+*/
+u_result RPLidar::startScan(bool force, _u32 timeout)
+{
+    //pc->printf("RPLidar::startScan\n");
+    u_result ans;
+
+//    if (!isOpen()) return RESULT_OPERATION_FAIL;
+
+    stop(); //force the previous operation to stop
+
+
+        ans = _sendCommand(force?RPLIDAR_CMD_FORCE_SCAN:RPLIDAR_CMD_SCAN, NULL, 0);
+        if (IS_FAIL(ans)) return ans;
+
+        // waiting for confirmation
+        rplidar_ans_header_t response_header;
+        if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
+            return ans;
+        }
+
+        // verify whether we got a correct header
+        if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT) {
+            return RESULT_INVALID_DATA;
+        }
+
+        if (response_header.size < sizeof(rplidar_response_measurement_node_t)) {
+            return RESULT_INVALID_DATA;
+        }
+        
+
+    return RESULT_OK;
+}
+
+// wait for one sample point to arrive
+/*
+    Do not use if startScan was called with starThread == 1!
+*/
+u_result RPLidar::waitPoint(_u32 timeout)
+{
+   
+   if(useThread)
+        return RESULT_OPERATION_NOT_SUPPORT;
+        
+   _u32 currentTs = timers.read_ms();
+   _u32 remainingtime;
+   rplidar_response_measurement_node_t node;
+   _u8 *nodebuf = (_u8*)&node;
+
+   _u8 recvPos = 0;
+
+   while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
+        int currentbyte = _bined_serialdev->getc();
+        if (currentbyte<0) continue;
+//Serial.println(currentbyte);
+        switch (recvPos) {
+            case 0: // expect the sync bit and its reverse in this byte          {
+                {
+                    _u8 tmp = (currentbyte>>1);
+                    if ( (tmp ^ currentbyte) & 0x1 ) {
+                        // pass
+                    } else {
+                        continue;
+                    }
+
+                }
+                break;
+            case 1: // expect the highest bit to be 1
+                {
+                    if (currentbyte & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) {
+                        // pass
+                    } else {
+                        recvPos = 0;
+                        continue;
+                    }
+                }
+                break;
+          }
+          nodebuf[recvPos++] = currentbyte;
+          if (recvPos == sizeof(rplidar_response_measurement_node_t)) {
+              // store the data ...
+              _currentMeasurement.distance = node.distance_q2/4.0f;
+              _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;
+              dis = _currentMeasurement.distance;
+
+
+              if(ang>=angMin&&ang<=angMax)Data[ang] = dis;
+
+              return RESULT_OK;
+          }
+
+
+   }
+
+   return RESULT_OPERATION_TIMEOUT;
+}
+
+
+/*
+    This is very similar to waitPoint and it's only to be used by the thread, 
+    hence why it private.
+    
+    It checks for data in the buffered serial until it finds a message or there
+    are no more bytes. 
+    If it finds a message it returns RESULT_OK
+    If there are no more bytes it returns RESULT_OPERATION_TIMEOUT.
+    
+    The state of the parsing is saved so it can continue parsing a measurement
+    midway. (Note that does not mean it's re-entrant and should only be used
+    in the same context)
+*/
+u_result RPLidar::pollPoint()
+{
+   
+   //_u32 currentTs = timers.read_ms();
+   _u32 remainingtime;
+   //rplidar_response_measurement_node_t node;
+   _u8 *nodebuf = (_u8*)&(node_g);
+
+
+   int currentbyte;
+   while(_bined_serialdev->readable() > 0)
+   {
+   //while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
+        currentbyte = _bined_serialdev->getc();
+        //if (_bined_serialdev->readable() == 0) continue; 
+        //Serial.println(currentbyte);
+        switch (recvPos_g) {
+            case 0: // expect the sync bit and its reverse in this byte          {
+                {
+                    _u8 tmp = (currentbyte>>1);
+                    if ( (tmp ^ currentbyte) & 0x1 ) {
+                        // pass
+                        
+                    } else {
+                        continue;
+                    }
+
+                }
+                break;
+            case 1: // expect the highest bit to be 1
+                {
+                    if (currentbyte & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) {
+                        // pass
+                    } else {
+                        recvPos_g = 0;
+                        continue;
+                    }
+                }
+                break;
+          }
+          nodebuf[recvPos_g++] = currentbyte;
+          if (recvPos_g == sizeof(rplidar_response_measurement_node_t)) {
+              recvPos_g = 0;
+              // store the data ...
+              _currentMeasurement.distance = node_g.distance_q2/4.0f;
+              _currentMeasurement.angle = (node_g.angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
+              _currentMeasurement.quality = (node_g.sync_quality>>RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
+              _currentMeasurement.startBit = (node_g.sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT);
+              ang = _currentMeasurement.angle;
+              dis = _currentMeasurement.distance;
+
+
+              if(ang>=angMin&&ang<=angMax)Data[ang] = dis;
+
+              return RESULT_OK;
+          }
+
+
+   }//while(_bined_serialdev->readable() > 0);
+
+   return RESULT_OPERATION_TIMEOUT;
+}
+
+
+void RPLidar::setAngle(int min,int max){
+  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)
+{
+
+    rplidar_cmd_packet_t pkt_header;
+    rplidar_cmd_packet_t * header = &pkt_header;
+    _u8 checksum = 0;
+
+    if (payloadsize && payload) {
+        cmd |= RPLIDAR_CMDFLAG_HAS_PAYLOAD;
+    }
+
+    header->syncByte = RPLIDAR_CMD_SYNC_BYTE;
+    header->cmd_flag = cmd;
+
+    // send header first
+    _bined_serialdev->putc(header->syncByte );
+    _bined_serialdev->putc(header->cmd_flag );
+
+  //  _bined_serialdev->write( (uint8_t *)header, 2);
+
+    if (cmd & RPLIDAR_CMDFLAG_HAS_PAYLOAD) {
+        checksum ^= RPLIDAR_CMD_SYNC_BYTE;
+        checksum ^= cmd;
+        checksum ^= (payloadsize & 0xFF);
+
+        // calc checksum
+        for (size_t pos = 0; pos < payloadsize; ++pos) {
+            checksum ^= ((_u8 *)payload)[pos];
+        }
+
+        // send size
+        _u8 sizebyte = payloadsize;
+        _bined_serialdev->putc(sizebyte);
+      //  _bined_serialdev->write((uint8_t *)&sizebyte, 1);
+
+        // send payload
+    //    _bined_serialdev.putc((uint8_t )payload);
+    //    _bined_serialdev->write((uint8_t *)&payload, sizebyte);
+
+        // send checksum
+        _bined_serialdev->putc(checksum);
+      //  _bined_serialdev->write((uint8_t *)&checksum, 1);
+
+    }
+
+    return RESULT_OK;
+}
+
+u_result RPLidar::_waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout)
+{
+    _u8  recvPos = 0;
+    _u32 currentTs = timers.read_ms();
+    _u32 remainingtime;
+    _u8 *headerbuf = (_u8*)header;
+    while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
+
+        int currentbyte = _bined_serialdev->getc();
+        if (currentbyte<0) continue;
+        switch (recvPos) {
+        case 0:
+            if (currentbyte != RPLIDAR_ANS_SYNC_BYTE1) {
+                continue;
+            }
+            break;
+        case 1:
+            if (currentbyte != RPLIDAR_ANS_SYNC_BYTE2) {
+                recvPos = 0;
+                continue;
+            }
+            break;
+        }
+        headerbuf[recvPos++] = currentbyte;
+
+        if (recvPos == sizeof(rplidar_ans_header_t)) {
+            return RESULT_OK;
+        }
+
+
+    }
+
+    return RESULT_OPERATION_TIMEOUT;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rplidar/rplidar.h	Tue Apr 09 17:53:31 2019 +0000
@@ -0,0 +1,114 @@
+/*
+ * RoboPeak RPLIDAR Driver for Arduino
+ * RoboPeak.com
+ *
+ * Copyright (c) 2014, RoboPeak
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
+ * SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+ * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+ * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+#include "mbed.h"
+#include "inc/rptypes.h"
+#include "inc/rplidar_cmd.h"
+#include <BufferedSerial.h>
+//#include "../BufferedSerial/BufferedSerial.h"
+struct RPLidarMeasurement
+{
+    float distance;
+    float angle;
+    uint8_t quality;
+    bool  startBit;
+};
+
+class RPLidar
+{
+public:
+    enum {
+        RPLIDAR_SERIAL_BAUDRATE = 115200,
+        RPLIDAR_DEFAULT_TIMEOUT = 500,
+    };
+
+    void Thread_readSensor(void);
+    int32_t pollSensorData(struct RPLidarMeasurement *_data);
+    RPLidar(Serial *_pc);
+    RPLidar();
+    ~RPLidar();
+
+    // open the given serial interface and try to connect to the RPLIDAR
+    //bool begin(BufferedSerial &serialobj);
+    void begin(BufferedSerial &serialobj);
+    // close the currently opened serial interface
+    void end();
+
+    // check whether the serial interface is opened
+  //  bool isOpen();
+
+    // ask the RPLIDAR for its health info
+    u_result getHealth(rplidar_response_device_health_t & healthinfo, _u32 timeout = RPLIDAR_DEFAULT_TIMEOUT);
+
+    // ask the RPLIDAR for its device info like the serial number
+    u_result getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout = RPLIDAR_DEFAULT_TIMEOUT);
+
+    // stop the measurement operation
+    u_result stop();
+
+    // start the measurement operation
+    u_result startThreadScan();
+    u_result startScan(bool force = true, _u32 timeout = RPLIDAR_DEFAULT_TIMEOUT*2);
+
+
+    // wait for one sample point to arrive
+    u_result waitPoint(_u32 timeout = RPLIDAR_DEFAULT_TIMEOUT);
+
+    u_result _sendCommand(_u8 cmd, const void * payload, size_t payloadsize);
+    // retrieve currently received sample point
+    int Data[360];
+    int ang,dis;
+    int angMin,angMax;
+    void setAngle(int min,int max);
+    void get_lidar();
+    const RPLidarMeasurement & getCurrentPoint()
+    {
+        return _currentMeasurement;
+    }
+
+private:
+    _u8 recvPos_g;
+    rplidar_response_measurement_node_t node_g;
+    
+     //Thread *thread_p;
+    Thread thread;
+    Mail<struct RPLidarMeasurement, 100> mail_box; //
+    bool useThread;
+    
+    u_result pollPoint();
+    //Serial *pc;
+protected:
+//    u_result _sendCommand(_u8 cmd, const void * payload, size_t payloadsize);
+    u_result _waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout);
+
+protected:
+    BufferedSerial * _bined_serialdev;
+    RPLidarMeasurement _currentMeasurement;
+};