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()
00064 {
00065 //BufferedSerial lidar_serial(PinName PA_11, PinName PA_12);
00066     //Serial.begin(115200);
00067     timers.start();
00068     _bined_serialdev.baud(115200);
00069 }
00070 // close the currently opened serial interface
00071 void RPLidar::end()
00072 {/*
00073     if (isOpen()) {
00074        _bined_serialdev->end();
00075        _bined_serialdev = NULL;
00076     }*/
00077 }
00078 
00079 
00080 // check whether the serial interface is opened
00081 /*
00082 bool RPLidar::isOpen()
00083 {
00084     return _bined_serialdev?true:false;
00085 }
00086 */
00087 // ask the RPLIDAR for its health info
00088 
00089 u_result RPLidar::getHealth(rplidar_response_device_health_t & healthinfo, _u32 timeout)
00090 {
00091     _u32 currentTs = timers.read_ms();
00092     _u32 remainingtime;
00093 
00094     _u8 *infobuf = (_u8 *)&healthinfo;
00095     _u8 recvPos = 0;
00096 
00097     rplidar_ans_header_t response_header;
00098     u_result  ans;
00099 
00100 
00101   //  if (!isOpen()) return RESULT_OPERATION_FAIL;
00102 
00103     {
00104         if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_HEALTH, NULL, 0))) {
00105             return ans;
00106         }
00107 
00108         if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
00109             return ans;
00110         }
00111 
00112         // verify whether we got a correct header
00113         if (response_header.type != RPLIDAR_ANS_TYPE_DEVHEALTH) {
00114             return RESULT_INVALID_DATA;
00115         }
00116 
00117         if ((response_header.size) < sizeof(rplidar_response_device_health_t)) {
00118             return RESULT_INVALID_DATA;
00119         }
00120 
00121         while ((remainingtime=timers.read_ms() - currentTs) <= timeout) {
00122             int currentbyte = _bined_serialdev.getc();
00123             if (currentbyte < 0) continue;
00124 
00125             infobuf[recvPos++] = currentbyte;
00126 
00127             if (recvPos == sizeof(rplidar_response_device_health_t)) {
00128                 return RESULT_OK;
00129             }
00130         }
00131     }
00132     return RESULT_OPERATION_TIMEOUT;
00133 }
00134 // ask the RPLIDAR for its device info like the serial number
00135 u_result RPLidar::getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout )
00136 {
00137     _u8  recvPos = 0;
00138     _u32 currentTs = timers.read_ms();
00139     _u32 remainingtime;
00140     _u8 *infobuf = (_u8*)&info;
00141     rplidar_ans_header_t response_header;
00142     u_result  ans;
00143 
00144   //  if (!isOpen()) return RESULT_OPERATION_FAIL;
00145 
00146     {
00147         if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_INFO,NULL,0))) {
00148             return ans;
00149         }
00150 
00151         if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
00152             return ans;
00153         }
00154 
00155         // verify whether we got a correct header
00156         if (response_header.type != RPLIDAR_ANS_TYPE_DEVINFO) {
00157             return RESULT_INVALID_DATA;
00158         }
00159 
00160         if (response_header.size < sizeof(rplidar_response_device_info_t)) {
00161             return RESULT_INVALID_DATA;
00162         }
00163 
00164         while ((remainingtime=timers.read_ms() - currentTs) <= timeout) {
00165             int currentbyte = _bined_serialdev.getc();
00166             if (currentbyte<0) continue;
00167             infobuf[recvPos++] = currentbyte;
00168 
00169             if (recvPos == sizeof(rplidar_response_device_info_t)) {
00170                 return RESULT_OK;
00171             }
00172         }
00173     }
00174 
00175     return RESULT_OPERATION_TIMEOUT;
00176 }
00177 
00178 // stop the measurement operation
00179 u_result RPLidar::stop()
00180 {
00181 //    if (!isOpen()) return RESULT_OPERATION_FAIL;
00182     u_result ans = _sendCommand(RPLIDAR_CMD_STOP,NULL,0);
00183     return ans;
00184 }
00185 
00186 // start the measurement operation
00187 u_result RPLidar::startScan(bool force, _u32 timeout)
00188 {
00189   /*
00190   rplidar_cmd_packet_t pkt_header;
00191   rplidar_cmd_packet_t * header = &pkt_header;
00192   header->syncByte = 0xA5;
00193   header->cmd_flag = 0x21;
00194   //se.write(12);
00195 
00196   _bined_serialdev.putc(header->syncByte );
00197   _bined_serialdev.putc(header->cmd_flag );
00198   */
00199 
00200     u_result ans;
00201 
00202 //    if (!isOpen()) return RESULT_OPERATION_FAIL;
00203 
00204     stop(); //force the previous operation to stop
00205 
00206 
00207         ans = _sendCommand(force?RPLIDAR_CMD_FORCE_SCAN:RPLIDAR_CMD_SCAN, NULL, 0);
00208         if (IS_FAIL(ans)) return ans;
00209 
00210         // waiting for confirmation
00211         rplidar_ans_header_t response_header;
00212         if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
00213             return ans;
00214         }
00215 
00216         // verify whether we got a correct header
00217         if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT) {
00218             return RESULT_INVALID_DATA;
00219         }
00220 
00221         if (response_header.size < sizeof(rplidar_response_measurement_node_t)) {
00222             return RESULT_INVALID_DATA;
00223         }
00224 
00225     return RESULT_OK;
00226 }
00227 
00228 // wait for one sample point to arrive
00229 u_result RPLidar::waitPoint(_u32 timeout)
00230 {
00231    _u32 currentTs = timers.read_ms();
00232    _u32 remainingtime;
00233    rplidar_response_measurement_node_t node;
00234    _u8 *nodebuf = (_u8*)&node;
00235 
00236    _u8 recvPos = 0;
00237 
00238    while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
00239         int currentbyte = _bined_serialdev.getc();
00240         if (currentbyte<0) continue;
00241 //Serial.println(currentbyte);
00242         switch (recvPos) {
00243             case 0: // expect the sync bit and its reverse in this byte          {
00244                 {
00245                     _u8 tmp = (currentbyte>>1);
00246                     if ( (tmp ^ currentbyte) & 0x1 ) {
00247                         // pass
00248                     } else {
00249                         continue;
00250                     }
00251 
00252                 }
00253                 break;
00254             case 1: // expect the highest bit to be 1
00255                 {
00256                     if (currentbyte & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) {
00257                         // pass
00258                     } else {
00259                         recvPos = 0;
00260                         continue;
00261                     }
00262                 }
00263                 break;
00264           }
00265           nodebuf[recvPos++] = currentbyte;
00266           if (recvPos == sizeof(rplidar_response_measurement_node_t)) {
00267               // store the data ...
00268               _currentMeasurement.distance = node.distance_q2/4.0f;
00269               _currentMeasurement.angle = (node.angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
00270               _currentMeasurement.quality = (node.sync_quality>>RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
00271               _currentMeasurement.startBit = (node.sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT);
00272               return RESULT_OK;
00273           }
00274 
00275 
00276    }
00277 
00278    return RESULT_OPERATION_TIMEOUT;
00279 }
00280 
00281 
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 }