RPlidar.cpp

00001 /*
00002  * RoboPeak RPLIDAR Driver for Arduino
00003  * RoboPeak.com
00004  *
00005  * Copyright (c) 2014, RoboPeak
00006  * All rights reserved.
00007  *
00008  * Redistribution and use in source and binary forms, with or without modification,
00009  * are permitted provided that the following conditions are met:
00010  *
00011  * 1. Redistributions of source code must retain the above copyright notice,
00012  *    this list of conditions and the following disclaimer.
00013  *
00014  * 2. Redistributions in binary form must reproduce the above copyright notice,
00015  *    this list of conditions and the following disclaimer in the documentation
00016  *    and/or other materials provided with the distribution.
00017  *
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
00019  * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
00020  * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
00021  * SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
00022  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
00023  * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
00024  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
00025  * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
00026  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027  *
00028  */
00029 
00030 #include "rplidar.h"
00031 //BufferedSerial _bined_serialdev( PA_11,  PA_12); // tx, rx
00032 Timer timers;
00033 RPLidar::RPLidar()
00034 {
00035 
00036     _currentMeasurement.distance = 0;
00037     _currentMeasurement.angle = 0;
00038     _currentMeasurement.quality = 0;
00039     _currentMeasurement.startBit = 0;
00040 }
00041 
00042 
00043 RPLidar::~RPLidar()
00044 {
00045     end();
00046 }
00047 
00048 // open the given serial interface and try to connect to the RPLIDAR
00049 /*
00050 bool RPLidar::begin(BufferedSerial &serialobj)
00051 {
00052 
00053     //Serial.begin(115200);
00054 
00055     if (isOpen()) {
00056       end();
00057     }
00058     _bined_serialdev = &serialobj;
00059   //  _bined_serialdev->end();
00060     _bined_serialdev->baud(RPLIDAR_SERIAL_BAUDRATE);
00061 }
00062 */
00063 void RPLidar::begin(BufferedSerial &serialobj)
00064 {
00065     _bined_serialdev = &serialobj;
00066     timers.start();
00067     _bined_serialdev->baud(RPLIDAR_SERIAL_BAUDRATE);
00068 }
00069 // close the currently opened serial interface
00070 void RPLidar::end()
00071 {/*
00072     if (isOpen()) {
00073        _bined_serialdev->end();
00074        _bined_serialdev = NULL;
00075     }*/
00076 }
00077 
00078 
00079 // check whether the serial interface is opened
00080 /*
00081 bool RPLidar::isOpen()
00082 {
00083     return _bined_serialdev?true:false;
00084 }
00085 */
00086 // ask the RPLIDAR for its health info
00087 
00088 u_result RPLidar::getHealth(rplidar_response_device_health_t & healthinfo, _u32 timeout)
00089 {
00090     _u32 currentTs = timers.read_ms();
00091     _u32 remainingtime;
00092 
00093     _u8 *infobuf = (_u8 *)&healthinfo;
00094     _u8 recvPos = 0;
00095 
00096     rplidar_ans_header_t response_header;
00097     u_result  ans;
00098 
00099 
00100   //  if (!isOpen()) return RESULT_OPERATION_FAIL;
00101 
00102     {
00103         if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_HEALTH, NULL, 0))) {
00104             return ans;
00105         }
00106 
00107         if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
00108             return ans;
00109         }
00110 
00111         // verify whether we got a correct header
00112         if (response_header.type != RPLIDAR_ANS_TYPE_DEVHEALTH) {
00113             return RESULT_INVALID_DATA;
00114         }
00115 
00116         if ((response_header.size) < sizeof(rplidar_response_device_health_t)) {
00117             return RESULT_INVALID_DATA;
00118         }
00119 
00120         while ((remainingtime=timers.read_ms() - currentTs) <= timeout) {
00121             int currentbyte = _bined_serialdev->getc();
00122             if (currentbyte < 0) continue;
00123 
00124             infobuf[recvPos++] = currentbyte;
00125 
00126             if (recvPos == sizeof(rplidar_response_device_health_t)) {
00127                 return RESULT_OK;
00128             }
00129         }
00130     }
00131     return RESULT_OPERATION_TIMEOUT;
00132 }
00133 // ask the RPLIDAR for its device info like the serial number
00134 u_result RPLidar::getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout )
00135 {
00136     _u8  recvPos = 0;
00137     _u32 currentTs = timers.read_ms();
00138     _u32 remainingtime;
00139     _u8 *infobuf = (_u8*)&info;
00140     rplidar_ans_header_t response_header;
00141     u_result  ans;
00142 
00143   //  if (!isOpen()) return RESULT_OPERATION_FAIL;
00144 
00145     {
00146         if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_INFO,NULL,0))) {
00147             return ans;
00148         }
00149 
00150         if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
00151             return ans;
00152         }
00153 
00154         // verify whether we got a correct header
00155         if (response_header.type != RPLIDAR_ANS_TYPE_DEVINFO) {
00156             return RESULT_INVALID_DATA;
00157         }
00158 
00159         if (response_header.size < sizeof(rplidar_response_device_info_t)) {
00160             return RESULT_INVALID_DATA;
00161         }
00162 
00163         while ((remainingtime=timers.read_ms() - currentTs) <= timeout) {
00164             int currentbyte = _bined_serialdev->getc();
00165             if (currentbyte<0) continue;
00166             infobuf[recvPos++] = currentbyte;
00167 
00168             if (recvPos == sizeof(rplidar_response_device_info_t)) {
00169                 return RESULT_OK;
00170             }
00171         }
00172     }
00173 
00174     return RESULT_OPERATION_TIMEOUT;
00175 }
00176 
00177 // stop the measurement operation
00178 u_result RPLidar::stop()
00179 {
00180 //    if (!isOpen()) return RESULT_OPERATION_FAIL;
00181     u_result ans = _sendCommand(RPLIDAR_CMD_STOP,NULL,0);
00182     return ans;
00183 }
00184 
00185 // start the measurement operation
00186 u_result RPLidar::startScan(bool force, _u32 timeout)
00187 {
00188     u_result ans;
00189 
00190 //    if (!isOpen()) return RESULT_OPERATION_FAIL;
00191 
00192     stop(); //force the previous operation to stop
00193 
00194 
00195         ans = _sendCommand(force?RPLIDAR_CMD_FORCE_SCAN:RPLIDAR_CMD_SCAN, NULL, 0);
00196         if (IS_FAIL(ans)) return ans;
00197 
00198         // waiting for confirmation
00199         rplidar_ans_header_t response_header;
00200         if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
00201             return ans;
00202         }
00203 
00204         // verify whether we got a correct header
00205         if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT) {
00206             return RESULT_INVALID_DATA;
00207         }
00208 
00209         if (response_header.size < sizeof(rplidar_response_measurement_node_t)) {
00210             return RESULT_INVALID_DATA;
00211         }
00212 
00213     return RESULT_OK;
00214 }
00215 
00216 // wait for one sample point to arrive
00217 u_result RPLidar::waitPoint(_u32 timeout)
00218 {
00219    _u32 currentTs = timers.read_ms();
00220    _u32 remainingtime;
00221    rplidar_response_measurement_node_t node;
00222    _u8 *nodebuf = (_u8*)&node;
00223 
00224    _u8 recvPos = 0;
00225 
00226    while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
00227         int currentbyte = _bined_serialdev->getc();
00228         if (currentbyte<0) continue;
00229 //Serial.println(currentbyte);
00230         switch (recvPos) {
00231             case 0: // expect the sync bit and its reverse in this byte          {
00232                 {
00233                     _u8 tmp = (currentbyte>>1);
00234                     if ( (tmp ^ currentbyte) & 0x1 ) {
00235                         // pass
00236                     } else {
00237                         continue;
00238                     }
00239 
00240                 }
00241                 break;
00242             case 1: // expect the highest bit to be 1
00243                 {
00244                     if (currentbyte & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) {
00245                         // pass
00246                     } else {
00247                         recvPos = 0;
00248                         continue;
00249                     }
00250                 }
00251                 break;
00252           }
00253           nodebuf[recvPos++] = currentbyte;
00254           if (recvPos == sizeof(rplidar_response_measurement_node_t)) {
00255               // store the data ...
00256               _currentMeasurement.distance = node.distance_q2/4.0f;
00257               _currentMeasurement.angle = (node.angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
00258               _currentMeasurement.quality = (node.sync_quality>>RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
00259               _currentMeasurement.startBit = (node.sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT);
00260               ang = _currentMeasurement.angle;
00261               dis = _currentMeasurement.distance;
00262 
00263 
00264               if(ang>=angMin&&ang<=angMax)Data[ang] = dis;
00265 
00266               return RESULT_OK;
00267           }
00268 
00269 
00270    }
00271 
00272    return RESULT_OPERATION_TIMEOUT;
00273 }
00274 
00275 
00276 void RPLidar::setAngle(int min,int max){
00277   angMin = min;
00278   angMax = max;
00279 }
00280 void RPLidar::get_lidar(){
00281   if (!IS_OK(waitPoint())) startScan();
00282 }
00283 u_result RPLidar::_sendCommand(_u8 cmd, const void * payload, size_t payloadsize)
00284 {
00285 
00286     rplidar_cmd_packet_t pkt_header;
00287     rplidar_cmd_packet_t * header = &pkt_header;
00288     _u8 checksum = 0;
00289 
00290     if (payloadsize && payload) {
00291         cmd |= RPLIDAR_CMDFLAG_HAS_PAYLOAD;
00292     }
00293 
00294     header->syncByte = RPLIDAR_CMD_SYNC_BYTE;
00295     header->cmd_flag = cmd;
00296 
00297     // send header first
00298     _bined_serialdev->putc(header->syncByte );
00299     _bined_serialdev->putc(header->cmd_flag );
00300 
00301   //  _bined_serialdev->write( (uint8_t *)header, 2);
00302 
00303     if (cmd & RPLIDAR_CMDFLAG_HAS_PAYLOAD) {
00304         checksum ^= RPLIDAR_CMD_SYNC_BYTE;
00305         checksum ^= cmd;
00306         checksum ^= (payloadsize & 0xFF);
00307 
00308         // calc checksum
00309         for (size_t pos = 0; pos < payloadsize; ++pos) {
00310             checksum ^= ((_u8 *)payload)[pos];
00311         }
00312 
00313         // send size
00314         _u8 sizebyte = payloadsize;
00315         _bined_serialdev->putc(sizebyte);
00316       //  _bined_serialdev->write((uint8_t *)&sizebyte, 1);
00317 
00318         // send payload
00319     //    _bined_serialdev.putc((uint8_t )payload);
00320     //    _bined_serialdev->write((uint8_t *)&payload, sizebyte);
00321 
00322         // send checksum
00323         _bined_serialdev->putc(checksum);
00324       //  _bined_serialdev->write((uint8_t *)&checksum, 1);
00325 
00326     }
00327 
00328     return RESULT_OK;
00329 }
00330 
00331 u_result RPLidar::_waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout)
00332 {
00333     _u8  recvPos = 0;
00334     _u32 currentTs = timers.read_ms();
00335     _u32 remainingtime;
00336     _u8 *headerbuf = (_u8*)header;
00337     while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
00338 
00339         int currentbyte = _bined_serialdev->getc();
00340         if (currentbyte<0) continue;
00341         switch (recvPos) {
00342         case 0:
00343             if (currentbyte != RPLIDAR_ANS_SYNC_BYTE1) {
00344                 continue;
00345             }
00346             break;
00347         case 1:
00348             if (currentbyte != RPLIDAR_ANS_SYNC_BYTE2) {
00349                 recvPos = 0;
00350                 continue;
00351             }
00352             break;
00353         }
00354         headerbuf[recvPos++] = currentbyte;
00355 
00356         if (recvPos == sizeof(rplidar_ans_header_t)) {
00357             return RESULT_OK;
00358         }
00359 
00360 
00361     }
00362 
00363     return RESULT_OPERATION_TIMEOUT;
00364 }