Diff: rplidar/RPlidar.cpp

Revision:

2:f873deba2305

Child:

4:de5a65c17664

diff -r 45f1573d65a1 -r f873deba2305 rplidar/RPlidar.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rplidar/RPlidar.cpp	Tue May 24 10:25:10 2016 +0000
@@ -0,0 +1,364 @@
+/*
+ * 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;
+RPLidar::RPLidar()
+{
+
+    _currentMeasurement.distance = 0;
+    _currentMeasurement.angle = 0;
+    _currentMeasurement.quality = 0;
+    _currentMeasurement.startBit = 0;
+}
+
+
+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 lidar_serial(PinName PA_11, PinName PA_12);
+    //Serial.begin(115200);
+    timers.start();
+    _bined_serialdev.baud(115200);
+}
+// 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;
+}
+
+// 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;
+
+    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
+u_result RPLidar::waitPoint(_u32 timeout)
+{
+   _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);
+              return RESULT_OK;
+          }
+
+
+   }
+
+   return RESULT_OPERATION_TIMEOUT;
+}
+
+
+
+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;
+}