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