sra-romi
Dependencies: BufferedSerial Matrix
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; +};