rplidar.cpp

00001 /*
00002  *  RPLIDAR SDK for Mbed
00003  *
00004  *  Copyright (c) 2009 - 2014 RoboPeak Team
00005  *  http://www.robopeak.com
00006  *  Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd.
00007  *  http://www.slamtec.com
00008  *
00009  */
00010 /*
00011  * Redistribution and use in source and binary forms, with or without 
00012  * modification, are permitted provided that the following conditions are met:
00013  *
00014  * 1. Redistributions of source code must retain the above copyright notice, 
00015  *    this list of conditions and the following disclaimer.
00016  *
00017  * 2. Redistributions in binary form must reproduce the above copyright notice, 
00018  *    this list of conditions and the following disclaimer in the documentation 
00019  *    and/or other materials provided with the distribution.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
00022  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
00023  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
00024  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
00025  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
00026  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
00027  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
00028  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
00029  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
00030  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
00031  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00032  *
00033  */
00034 
00035 #include "rplidar.h"
00036 
00037 Timer timers;
00038 RPLidar::RPLidar()
00039 {
00040 
00041     _currentMeasurement.distance = 0;
00042     _currentMeasurement.angle = 0;
00043     _currentMeasurement.quality = 0;
00044     _currentMeasurement.startBit = 0;
00045 }
00046 
00047 
00048 RPLidar::~RPLidar()
00049 {
00050     end();
00051 }
00052 
00053 // open the given serial interface and try to connect to the RPLIDAR
00054 /*
00055 bool RPLidar::begin(BufferedSerial &serialobj)
00056 {
00057 
00058     //Serial.begin(115200);
00059 
00060     if (isOpen()) {
00061       end();
00062     }
00063     _binded_serialdev = &serialobj;
00064   //  _binded_serialdev->end();
00065     _binded_serialdev->baud(RPLIDAR_SERIAL_BAUDRATE);
00066 }
00067 */
00068 void RPLidar::begin(RawSerial& serialobj)
00069 {
00070     _binded_serialdev = &serialobj;
00071     timers.start();
00072     _binded_serialdev->baud(RPLIDAR_SERIAL_BAUDRATE);
00073 }
00074 // close the currently opened serial interface
00075 void RPLidar::end()
00076 {/*
00077     if (isOpen()) {
00078        _binded_serialdev->end();
00079        _binded_serialdev = NULL;
00080     }*/
00081 }
00082 
00083 
00084 // check whether the serial interface is opened
00085 /*
00086 bool RPLidar::isOpen()
00087 {
00088     return _binded_serialdev?true:false;
00089 }
00090 */
00091 // ask the RPLIDAR for its health info
00092 
00093 u_result RPLidar::getHealth(rplidar_response_device_health_t& healthinfo, uint32_t timeout)
00094 {
00095     uint32_t startTs = timers.read_ms();
00096     uint32_t remainingtime;
00097 
00098     uint8_t* infobuf = (uint8_t*)&healthinfo;
00099     uint8_t recvPos = 0;
00100 
00101     rplidar_ans_header_t response_header;
00102     u_result  ans;
00103 
00104 
00105     //  if (!isOpen()) return RESULT_OPERATION_FAIL;
00106 
00107     {
00108         if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_HEALTH, NULL, 0))) {
00109             return ans;
00110         }
00111 
00112         if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
00113             return ans;
00114         }
00115 
00116         // verify whether we got a correct header
00117         if (response_header.type != RPLIDAR_ANS_TYPE_DEVHEALTH) {
00118             return RESULT_INVALID_DATA;
00119         }
00120 
00121         if ((response_header.size) < sizeof(rplidar_response_device_health_t)) {
00122             return RESULT_INVALID_DATA;
00123         }
00124 
00125         while ((remainingtime = timers.read_ms() - startTs) <= timeout) {
00126             int currentbyte = _binded_serialdev->getc();
00127             if (currentbyte < 0) continue;
00128 
00129             infobuf[recvPos++] = currentbyte;
00130 
00131             if (recvPos == sizeof(rplidar_response_device_health_t)) {
00132                 return RESULT_OK;
00133             }
00134         }
00135     }
00136     return RESULT_OPERATION_TIMEOUT;
00137 }
00138 // ask the RPLIDAR for its device info like the serial number
00139 u_result RPLidar::getDeviceInfo(rplidar_response_device_info_t& info, uint32_t timeout)
00140 {
00141     uint8_t  recvPos = 0;
00142     uint32_t startTs = timers.read_ms();
00143     uint32_t remainingtime;
00144     uint8_t* infobuf = (uint8_t*)&info;
00145     rplidar_ans_header_t response_header;
00146     u_result  ans;
00147 
00148     //  if (!isOpen()) return RESULT_OPERATION_FAIL;f
00149 
00150     {
00151         if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_INFO, NULL, 0))) {
00152             return ans;
00153         }
00154 
00155         if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
00156             return ans;
00157         }
00158 
00159         // verify whether we got a correct header
00160         if (response_header.type != RPLIDAR_ANS_TYPE_DEVINFO) {
00161             return RESULT_INVALID_DATA;
00162         }
00163 
00164         if (response_header.size < sizeof(rplidar_response_device_info_t)) {
00165             return RESULT_INVALID_DATA;
00166         }
00167 
00168         while ((remainingtime = timers.read_ms() - startTs) <= timeout) {
00169             int currentbyte = _binded_serialdev->getc();
00170             if (currentbyte < 0) continue;
00171             infobuf[recvPos++] = currentbyte;
00172 
00173             if (recvPos == sizeof(rplidar_response_device_info_t)) {
00174                 return RESULT_OK;
00175             }
00176         }
00177     }
00178 
00179     return RESULT_OPERATION_TIMEOUT;
00180 }
00181 
00182 // stop the measurement operation
00183 u_result RPLidar::stop()
00184 {
00185     //    if (!isOpen()) return RESULT_OPERATION_FAIL;
00186     u_result ans = _sendCommand(RPLIDAR_CMD_STOP, NULL, 0);
00187     return ans;
00188 }
00189 
00190 // start the measurement operation
00191 u_result RPLidar::startScanNormal(bool force, uint32_t timeout)
00192 {
00193     u_result ans;
00194 
00195     //    if (!isOpen()) return RESULT_OPERATION_FAIL;
00196 
00197     stop(); //force the previous operation to stop
00198 
00199 
00200     ans = _sendCommand(force ? RPLIDAR_CMD_FORCE_SCAN : RPLIDAR_CMD_SCAN, NULL, 0);
00201     if (IS_FAIL(ans)) return ans;
00202 
00203     // waiting for confirmation
00204     rplidar_ans_header_t response_header;
00205     if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
00206         return ans;
00207     }
00208 
00209     // verify whether we got a correct header
00210     if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT) {
00211         return RESULT_INVALID_DATA;
00212     }
00213 
00214     if (response_header.size < sizeof(rplidar_response_measurement_node_t)) {
00215         return RESULT_INVALID_DATA;
00216     }
00217 
00218     return RESULT_OK;
00219 }
00220 
00221 u_result RPLidar::startScanExpress(bool force, uint16_t scanMode, uint32_t options, RplidarScanMode* outUsedScanMode, uint32_t timeout)
00222 {
00223     u_result ans;
00224     // if (!isConnected()) return RESULT_OPERATION_FAIL;
00225     // if (_isScanning) return RESULT_ALREADY_DONE;
00226 
00227     stop(); //force the previous operation to stop
00228 
00229     if (scanMode == RPLIDAR_CONF_SCAN_COMMAND_STD)
00230     {
00231         //return startScan(force, false, 0, outUsedScanMode);
00232     }
00233 
00234     
00235     bool ifSupportLidarConf = false;
00236     ans = checkSupportConfigCommands(ifSupportLidarConf);
00237     if (IS_FAIL(ans)) return RESULT_INVALID_DATA;
00238 
00239     if (outUsedScanMode)
00240     {
00241         outUsedScanMode->id = scanMode;
00242         if (ifSupportLidarConf)
00243         {
00244             ans = getLidarSampleDuration(outUsedScanMode->us_per_sample, outUsedScanMode->id);
00245             if (IS_FAIL(ans))
00246             {
00247                 return RESULT_INVALID_DATA;
00248             }
00249 
00250             ans = getMaxDistance(outUsedScanMode->max_distance, outUsedScanMode->id);
00251             if (IS_FAIL(ans))
00252             {
00253                 return RESULT_INVALID_DATA;
00254             }
00255 
00256             ans = getScanModeAnsType(outUsedScanMode->ans_type, outUsedScanMode->id);
00257             if (IS_FAIL(ans))
00258             {
00259                 return RESULT_INVALID_DATA;
00260             }
00261 
00262             ans = getScanModeName(outUsedScanMode->scan_mode, outUsedScanMode->id);
00263             if (IS_FAIL(ans))
00264             {
00265                 return RESULT_INVALID_DATA;
00266             }
00267            
00268 
00269         }
00270         else
00271         {
00272             rplidar_response_sample_rate_t sampleRateTmp;
00273             ans = getSampleDuration_uS(sampleRateTmp);
00274             if (IS_FAIL(ans)) return RESULT_INVALID_DATA;
00275 
00276             outUsedScanMode->us_per_sample = sampleRateTmp.express_sample_duration_us;
00277             outUsedScanMode->max_distance = 16;
00278             outUsedScanMode->ans_type = RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED;
00279             strcpy(outUsedScanMode->scan_mode, "Express");
00280         }
00281     }
00282 
00283     //get scan answer type to specify how to wait data
00284     uint8_t scanAnsType;
00285     if (ifSupportLidarConf)
00286     {      
00287         getScanModeAnsType(scanAnsType, scanMode);
00288     }
00289     else
00290     {
00291         scanAnsType = RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED;
00292     }
00293 
00294     {
00295         //rp::hal::AutoLocker l(_lock);
00296 
00297         rplidar_payload_express_scan_t scanReq;
00298         memset(&scanReq, 0, sizeof(scanReq));
00299         if (scanMode != RPLIDAR_CONF_SCAN_COMMAND_STD && scanMode != RPLIDAR_CONF_SCAN_COMMAND_EXPRESS)
00300             scanReq.working_mode = uint8_t(scanMode);
00301         scanReq.working_flags = options;
00302 
00303         if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_EXPRESS_SCAN, &scanReq, sizeof(scanReq)))) {
00304             return ans;
00305         }
00306 
00307         // waiting for confirmation
00308         rplidar_ans_header_t response_header;
00309         if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
00310             return ans;
00311         }
00312 
00313         // verify whether we got a correct header
00314         if (response_header.type != scanAnsType) {
00315             return RESULT_INVALID_DATA;
00316         }
00317 
00318         uint32_t header_size = (response_header.size & RPLIDAR_ANS_HEADER_SIZE_MASK);
00319 
00320         if (scanAnsType == RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED)
00321         {
00322             if (header_size < sizeof(rplidar_response_capsule_measurement_nodes_t)) {
00323                 return RESULT_INVALID_DATA;
00324             }
00325             _cached_express_flag = 0;
00326             _isScanning = true;
00327             //_cachethread = CLASS_THREAD(RPlidarDriverImplCommon, _cacheCapsuledScanData);
00328         }
00329         else if (scanAnsType == RPLIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED)
00330         {
00331             if (header_size < sizeof(rplidar_response_capsule_measurement_nodes_t)) {
00332                 return RESULT_INVALID_DATA;
00333             }
00334             _cached_express_flag = 1;
00335             _isScanning = true;
00336             //_cachethread = CLASS_THREAD(RPlidarDriverImplCommon, _cacheCapsuledScanData);
00337         }
00338         else if (scanAnsType == RPLIDAR_ANS_TYPE_MEASUREMENT_HQ) {
00339             if (header_size < sizeof(rplidar_response_hq_capsule_measurement_nodes_t)) {
00340                 return RESULT_INVALID_DATA;
00341             }
00342             _isScanning = true;
00343             //_cachethread = CLASS_THREAD(RPlidarDriverImplCommon, _cacheHqScanData);
00344         }
00345         else
00346         {
00347             if (header_size < sizeof(rplidar_response_ultra_capsule_measurement_nodes_t)) {
00348                 return RESULT_INVALID_DATA;
00349             }
00350             _isScanning = true;
00351             //_cachethread = CLASS_THREAD(RPlidarDriverImplCommon, _cacheUltraCapsuledScanData);
00352         }
00353 
00354         // if (_cachethread.getHandle() == 0) {
00355         //     return RESULT_OPERATION_FAIL;
00356         // }
00357     }
00358     return RESULT_OK;
00359 }
00360 
00361 u_result RPLidar::getScanModeAnsType(uint8_t &ansType, uint16_t scanModeID, uint32_t timeoutInMs)
00362 {
00363     u_result ans;
00364     std::vector<uint8_t> reserve(2);
00365     memcpy(&reserve[0], &scanModeID, sizeof(scanModeID));
00366 
00367     std::vector<uint8_t> answer;
00368     ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_ANS_TYPE, answer, reserve, timeoutInMs);
00369     if (IS_FAIL(ans))
00370     {
00371         return ans;
00372     }
00373     if (answer.size() < sizeof(uint8_t))
00374     {
00375         return RESULT_INVALID_DATA;
00376     }
00377     const uint8_t *result = reinterpret_cast<const uint8_t*>(&answer[0]);
00378     ansType = *result;
00379     return ans;
00380 }
00381 
00382 u_result RPLidar::getScanModeName(char* modeName, uint16_t scanModeID, uint32_t timeoutInMs)
00383 {
00384     u_result ans;
00385     std::vector<uint8_t> reserve(2);
00386     memcpy(&reserve[0], &scanModeID, sizeof(scanModeID));
00387 
00388     std::vector<uint8_t> answer;
00389     ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_NAME, answer, reserve, timeoutInMs);
00390     if (IS_FAIL(ans))
00391     {
00392         return ans;
00393     }
00394     int len = answer.size();
00395     if (0 == len) return RESULT_INVALID_DATA;
00396     memcpy(modeName, &answer[0], len);
00397     return ans;
00398 }
00399 
00400 u_result RPLidar::getAllSupportedScanModes(std::vector<RplidarScanMode>& outModes, uint32_t timeoutInMs)
00401 {
00402     u_result ans;
00403     bool confProtocolSupported = false;
00404     ans = checkSupportConfigCommands(confProtocolSupported);
00405     if (IS_FAIL(ans)) return RESULT_INVALID_DATA;
00406 
00407     if (confProtocolSupported)
00408     {
00409         // 1. get scan mode count
00410         uint16_t modeCount;
00411         ans = getScanModeCount(modeCount);
00412         if (IS_FAIL(ans))
00413         {
00414             return RESULT_INVALID_DATA;
00415         }
00416         // 2. for loop to get all fields of each scan mode
00417         for (uint16_t i = 0; i < modeCount; i++)
00418         {
00419             RplidarScanMode scanModeInfoTmp;
00420             memset(&scanModeInfoTmp, 0, sizeof(scanModeInfoTmp));
00421             scanModeInfoTmp.id = i;
00422             ans = getLidarSampleDuration(scanModeInfoTmp.us_per_sample, i);
00423             if (IS_FAIL(ans))
00424             {
00425                 return RESULT_INVALID_DATA;
00426             }
00427             ans = getMaxDistance(scanModeInfoTmp.max_distance, i);
00428             if (IS_FAIL(ans))
00429             {
00430                 return RESULT_INVALID_DATA;
00431             }
00432             ans = getScanModeAnsType(scanModeInfoTmp.ans_type, i);
00433             if (IS_FAIL(ans))
00434             {
00435                 return RESULT_INVALID_DATA;
00436             }
00437             ans = getScanModeName(scanModeInfoTmp.scan_mode, i);
00438             if (IS_FAIL(ans))
00439             {
00440                 return RESULT_INVALID_DATA;
00441             }
00442             outModes.push_back(scanModeInfoTmp);
00443         }
00444         return ans;
00445     }
00446     else
00447     {
00448         rplidar_response_sample_rate_t sampleRateTmp;
00449         ans = getSampleDuration_uS(sampleRateTmp);
00450         if (IS_FAIL(ans)) return RESULT_INVALID_DATA;
00451         //judge if support express scan
00452         bool ifSupportExpScan = false;
00453         ans = checkExpressScanSupported(ifSupportExpScan);
00454         if (IS_FAIL(ans)) return RESULT_INVALID_DATA;
00455 
00456         RplidarScanMode stdScanModeInfo;
00457         stdScanModeInfo.id = RPLIDAR_CONF_SCAN_COMMAND_STD;
00458         stdScanModeInfo.us_per_sample = sampleRateTmp.std_sample_duration_us;
00459         stdScanModeInfo.max_distance = 16;
00460         stdScanModeInfo.ans_type = RPLIDAR_ANS_TYPE_MEASUREMENT;
00461         strcpy(stdScanModeInfo.scan_mode, "Standard");
00462         outModes.push_back(stdScanModeInfo);
00463         if (ifSupportExpScan)
00464         {
00465             RplidarScanMode expScanModeInfo;
00466             expScanModeInfo.id = RPLIDAR_CONF_SCAN_COMMAND_EXPRESS;
00467             expScanModeInfo.us_per_sample = sampleRateTmp.express_sample_duration_us;
00468             expScanModeInfo.max_distance = 16;
00469             expScanModeInfo.ans_type = RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED;
00470             strcpy(expScanModeInfo.scan_mode, "Express");
00471             outModes.push_back(expScanModeInfo);
00472         }
00473         return ans;
00474     }
00475     return ans;
00476 }
00477 
00478 u_result RPLidar::getSampleDuration_uS(rplidar_response_sample_rate_t & rateInfo, uint32_t timeout)
00479 {  
00480     //DEPRECATED_WARN("getSampleDuration_uS", "RplidarScanMode::us_per_sample");
00481 
00482     //if (!isConnected()) return RESULT_OPERATION_FAIL;
00483     
00484     _disableDataGrabbing();
00485     
00486     rplidar_response_device_info_t devinfo;
00487     // 1. fetch the device version first...
00488     u_result ans = getDeviceInfo(devinfo, timeout);
00489 
00490     rateInfo.express_sample_duration_us = _cached_sampleduration_express;
00491     rateInfo.std_sample_duration_us = _cached_sampleduration_std;
00492 
00493     if (devinfo.firmware_version < ((0x1<<8) | 17)) {
00494         // provide fake data...
00495 
00496         return RESULT_OK;
00497     }
00498 
00499 
00500     {
00501         //rp::hal::AutoLocker l(_lock);
00502 
00503         if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_SAMPLERATE, NULL, 0))) {
00504             return ans;
00505         }
00506 
00507         rplidar_ans_header_t response_header;
00508         if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
00509             return ans;
00510         }
00511 
00512         // verify whether we got a correct header
00513         if (response_header.type != RPLIDAR_ANS_TYPE_SAMPLE_RATE) {
00514             return RESULT_INVALID_DATA;
00515         }
00516 
00517         uint32_t header_size = (response_header.size & RPLIDAR_ANS_HEADER_SIZE_MASK);
00518         if ( header_size < sizeof(rplidar_response_sample_rate_t)) {
00519             return RESULT_INVALID_DATA;
00520         }
00521 
00522         // if (!_chanDev->waitfordata(header_size, timeout)) {
00523         //     return RESULT_OPERATION_TIMEOUT;
00524         // }
00525         //_chanDev->recvdata(reinterpret_cast<uint8_t *>(&rateInfo), sizeof(rateInfo));
00526         for(int i=0;i<sizeof(rateInfo);i++)//please check again!!!!!!
00527             _binded_serialdev->putc(reinterpret_cast<uint8_t *>(&rateInfo)[i]);
00528     }
00529     return RESULT_OK;
00530 }
00531 u_result RPLidar::checkExpressScanSupported(bool & support, uint32_t timeout)
00532 {
00533     //DEPRECATED_WARN("checkExpressScanSupported(bool&,_u32)", "getAllSupportedScanModes()");
00534 
00535     rplidar_response_device_info_t devinfo;
00536 
00537     support = false;
00538     u_result ans = getDeviceInfo(devinfo, timeout);
00539 
00540     if (IS_FAIL(ans)) return ans;
00541 
00542     if (devinfo.firmware_version >= ((0x1<<8) | 17)) {
00543         support = true;
00544         rplidar_response_sample_rate_t sample_rate;
00545         getSampleDuration_uS(sample_rate);
00546         _cached_sampleduration_express = sample_rate.express_sample_duration_us;
00547         _cached_sampleduration_std = sample_rate.std_sample_duration_us;
00548     }
00549 
00550     return RESULT_OK;
00551 }
00552 u_result RPLidar::getScanModeCount(uint16_t& modeCount, uint32_t timeoutInMs)
00553 {
00554     u_result ans;
00555     std::vector<uint8_t> answer;
00556     ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_COUNT, answer, std::vector<uint8_t>(), timeoutInMs);
00557 
00558     if (IS_FAIL(ans)) {
00559         return ans;
00560     }
00561     if (answer.size() < sizeof(uint16_t)) {
00562         return RESULT_INVALID_DATA;
00563     }
00564     const uint16_t *p_answer = reinterpret_cast<const uint16_t*>(&answer[0]);
00565     modeCount = *p_answer;
00566 
00567     return ans;
00568 }
00569 
00570 void RPLidar::_disableDataGrabbing()
00571 {
00572     _isScanning = false;
00573     //_cachethread.join();
00574 }
00575 
00576 
00577 u_result RPLidar::getMaxDistance(float &maxDistance, uint16_t scanModeID, uint32_t timeoutInMs)
00578 {
00579     u_result ans;
00580     std::vector<uint8_t> reserve(2);
00581     memcpy(&reserve[0], &scanModeID, sizeof(scanModeID));
00582 
00583     std::vector<uint8_t> answer;
00584     ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_MAX_DISTANCE, answer, reserve, timeoutInMs);
00585     if (IS_FAIL(ans))
00586     {
00587         return ans;
00588     }
00589     if (answer.size() < sizeof(uint32_t))
00590     {
00591         return RESULT_INVALID_DATA;
00592     }
00593     const uint32_t *result = reinterpret_cast<const uint32_t*>(&answer[0]);
00594     maxDistance = (float)(*result >> 8);
00595     return ans;
00596 }
00597 
00598 u_result RPLidar::getLidarConf(uint32_t type, std::vector<uint8_t> &outputBuf, const std::vector<uint8_t> &reserve, uint32_t timeout)
00599 {
00600     rplidar_payload_get_scan_conf_t query;
00601     memset(&query, 0, sizeof(query));
00602     query.type = type;
00603     int sizeVec = reserve.size();
00604 
00605     int maxLen = sizeof(query.reserved) / sizeof(query.reserved[0]);
00606     if (sizeVec > maxLen) sizeVec = maxLen;
00607 
00608     if (sizeVec > 0)
00609         memcpy(query.reserved, &reserve[0], reserve.size());
00610 
00611     u_result ans;
00612     {
00613         //rp::hal::AutoLocker l(_lock);
00614         if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_LIDAR_CONF, &query, sizeof(query)))) {
00615             return ans;
00616         }
00617 
00618         // waiting for confirmation
00619         rplidar_ans_header_t response_header;
00620         if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
00621             return ans;
00622         }
00623 
00624         // verify whether we got a correct header
00625         if (response_header.type != RPLIDAR_ANS_TYPE_GET_LIDAR_CONF) {
00626             return RESULT_INVALID_DATA;
00627         }
00628 
00629         uint32_t header_size = (response_header.size & RPLIDAR_ANS_HEADER_SIZE_MASK);
00630         if (header_size < sizeof(type)) {
00631             return RESULT_INVALID_DATA;
00632         }
00633 
00634         // if (!_chanDev->waitfordata(header_size, timeout)) {
00635         //     return RESULT_OPERATION_TIMEOUT;
00636         // }
00637 
00638         std::vector<uint8_t> dataBuf;
00639         dataBuf.resize(header_size);
00640         //_chanDev->recvdata(reinterpret_cast<uint8_t *>(&dataBuf[0]), header_size);
00641         for(int i=0;i<header_size;i++)//please check again!!!!!!
00642             _binded_serialdev->putc(dataBuf[i]);
00643 
00644         //check if returned type is same as asked type
00645         uint32_t replyType = -1;
00646         memcpy(&replyType, &dataBuf[0], sizeof(type));
00647         if (replyType != type) {
00648             return RESULT_INVALID_DATA;
00649         }
00650 
00651         //copy all the payload into &outputBuf
00652         int payLoadLen = header_size - sizeof(type);
00653 
00654         //do consistency check
00655         if (payLoadLen <= 0) {
00656             return RESULT_INVALID_DATA;
00657         }
00658         //copy all payLoadLen bytes to outputBuf
00659         outputBuf.resize(payLoadLen);
00660         memcpy(&outputBuf[0], &dataBuf[0] + sizeof(type), payLoadLen);
00661     }
00662     return ans;
00663 }
00664 
00665 u_result RPLidar::checkSupportConfigCommands(bool& outSupport, uint32_t timeoutInMs)
00666 {
00667     u_result ans;
00668 
00669     rplidar_response_device_info_t devinfo;
00670     ans = getDeviceInfo(devinfo, timeoutInMs);
00671     if (IS_FAIL(ans)) return ans;
00672 
00673     // if lidar firmware >= 1.24
00674     if (devinfo.firmware_version >= ((0x1 << 8) | 24)) {
00675         outSupport = true;
00676     }
00677     return ans;
00678 }
00679 
00680 u_result RPLidar::getLidarSampleDuration(float& sampleDurationRes, uint16_t scanModeID, uint32_t timeoutInMs)
00681 {
00682     u_result ans;
00683     std::vector<uint8_t> reserve(2);
00684     memcpy(&reserve[0], &scanModeID, sizeof(scanModeID));
00685 
00686     std::vector<uint8_t> answer;
00687     ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_US_PER_SAMPLE, answer, reserve, timeoutInMs);
00688     if (IS_FAIL(ans))
00689     {
00690         return ans;
00691     }
00692     if (answer.size() < sizeof(uint32_t))
00693     {
00694         return RESULT_INVALID_DATA;
00695     }
00696     const uint32_t *result = reinterpret_cast<const uint32_t*>(&answer[0]);
00697     sampleDurationRes = (float)(*result >> 8);
00698     return ans;
00699 }
00700 
00701 // wait for one sample point to arrive
00702 u_result RPLidar::waitPoint(uint32_t timeout){
00703     uint8_t recvPos = 0;
00704     uint32_t startTs = timers.read_ms();
00705 
00706     uint32_t waitTime;
00707     rplidar_response_measurement_node_t node;
00708     uint8_t* nodebuf = (uint8_t*)&node;
00709 
00710     
00711     uint8_t currentByte;
00712     while ((waitTime = timers.read_ms() - startTs) <= timeout) {
00713         if(_binded_serialdev->readable()) 
00714             currentByte = _binded_serialdev->getc();
00715         else
00716             continue;
00717         
00718         switch (recvPos) {
00719         case 0: // expect the sync bit and its reverse in this byte          {
00720         {
00721             uint8_t tmp = (currentByte >> 1);
00722             if ((tmp ^ currentByte) & 0x1) {
00723                 // pass
00724             }
00725             else {
00726                 continue;
00727             }
00728 
00729         }
00730         break;
00731         case 1: // expect the Check bit to be 1
00732         {
00733             if (currentByte & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) {
00734                 // pass
00735             }
00736             else {
00737                 recvPos = 0;
00738                 continue;
00739             }
00740         }
00741         break;
00742         }
00743         nodebuf[recvPos++] = currentByte;
00744 
00745         if (recvPos == sizeof(rplidar_response_measurement_node_t)) {
00746             // store the data ...
00747             _currentMeasurement.distance = node.distance_q2 / 4.0f;
00748             _currentMeasurement.angle = (node.angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f;
00749             _currentMeasurement.quality = (node.sync_quality >> RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
00750             _currentMeasurement.startBit = (node.sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT);
00751             ang = (int)_currentMeasurement.angle;
00752             //dis = _currentMeasurement.distance;
00753 
00754             if (ang >= angMin && ang <= angMax) Data[ang] =  _currentMeasurement;
00755             //.distance;// (float) ang;
00756             //wait_ms(200);
00757            /* for(int i = 0; i < size_t(nodebuf); i++)
00758                 Data[i] = nodebuf[i]*/
00759             //if (ang >= angMin && ang <= angMax)Data[ang] = 12.0;//_currentMeasurement.distance;
00760 //continue;
00761             return RESULT_OK;
00762         }
00763 
00764 
00765     }
00766     //return RESULT_OK;     //debug
00767     return RESULT_OPERATION_TIMEOUT;
00768 }
00769 
00770 void RPLidar::setAngle(int min, int max) {
00771     angMin = min;
00772     angMax = max;
00773 }
00774 void RPLidar::setLidar() {
00775     if (!IS_OK(waitPoint())) {
00776         printf("Connection failed.\n");
00777         printf("Try to invert Rx and Tx.\n");
00778         startScanNormal();
00779         wait_ms(2'000);
00780         printf("Done \n");
00781         //startScanNormal(0);
00782         //startScanExpress(0,0);
00783         //wait_us(2000000);
00784     }
00785     else{
00786         printf("Connection succeeded.\n");
00787         startScanNormal(1);
00788         //wait_ms(2'000);*/
00789 
00790     }
00791 }
00792 u_result RPLidar::_sendCommand(uint8_t cmd, const void* payload, size_t payloadsize){
00793 
00794     rplidar_cmd_packet_t pkt_header;
00795     rplidar_cmd_packet_t* header = &pkt_header;
00796     uint8_t checksum = 0;
00797 
00798     if (payloadsize && payload) {
00799         cmd |= RPLIDAR_CMDFLAG_HAS_PAYLOAD;
00800     }
00801 
00802     header->syncByte = RPLIDAR_CMD_SYNC_BYTE;
00803     header->cmd_flag = cmd;
00804 
00805     // send header first
00806     //same as -----> _chanDev->senddata(pkt_header, 2) ;
00807     _binded_serialdev->putc(header->syncByte);
00808     _binded_serialdev->putc(header->cmd_flag);
00809 
00810     //  _binded_serialdev->write( (uint8_t *)header, 2);
00811 
00812     if (cmd & RPLIDAR_CMDFLAG_HAS_PAYLOAD) {
00813         checksum ^= RPLIDAR_CMD_SYNC_BYTE;
00814         checksum ^= cmd;
00815         checksum ^= (payloadsize & 0xFF);
00816 
00817         // calc checksum
00818         for (size_t pos = 0; pos < payloadsize; ++pos) {
00819             checksum ^= ((uint8_t*)payload)[pos];
00820         }
00821 
00822         // send size
00823         uint8_t sizebyte = payloadsize;
00824         //printf("%d %d !!\n",sizebyte  ,checksum);
00825         _binded_serialdev->putc(sizebyte);
00826         //  _binded_serialdev->write((uint8_t *)&sizebyte, 1);
00827 
00828           // send payload
00829         for(int i=0;i<sizebyte;i++)//please check again!!!!!!
00830             _binded_serialdev->putc(((uint8_t*)payload)[i]);
00831         //  _binded_serialdev->write((uint8_t *)&payload, sizebyte,  event_callback_t &callback);
00832         //_binded_serialdev->write(const uint8_t *buffer, int length, const event_callback_t &callback)
00833           
00834           // send checksum
00835         _binded_serialdev->putc(checksum);
00836         //  _binded_serialdev->write((uint8_t *)&checksum, 1);
00837 
00838     }
00839 
00840     return RESULT_OK;
00841 }
00842 
00843 u_result RPLidar::_waitResponseHeader(rplidar_ans_header_t* header, uint32_t timeout){
00844     uint8_t  recvPos = 0;
00845     uint32_t startTs = timers.read_ms();
00846     uint32_t remainingtime;
00847     uint8_t* headerbuf = (uint8_t*)header;
00848     // printf("%d   ",timers.read_ms());
00849     while (1) {//(timers.read_ms() - startTs) <= timeout
00850         uint8_t currentbyte = _binded_serialdev->getc();
00851         if (currentbyte < 0) continue;
00852         switch (recvPos) {
00853         case 0:
00854             if (currentbyte != RPLIDAR_ANS_SYNC_BYTE1) {
00855                 continue;
00856             }
00857             break;
00858         case 1:
00859             if (currentbyte != RPLIDAR_ANS_SYNC_BYTE2) {
00860                 recvPos = 0;
00861                 continue;
00862             }
00863             break;
00864         }
00865         headerbuf[recvPos++] = currentbyte;
00866 
00867         if (recvPos == sizeof(rplidar_ans_header_t)) {
00868             return RESULT_OK;
00869         }
00870 
00871 
00872     }
00873     printf("RESULT_OPERATION_TIMEOUT 11\n");
00874     return RESULT_OPERATION_TIMEOUT;
00875 }
00876 
00877 
00878 // RPLidar::RPLidar()
00879 //     : _isConnected(false)
00880 //     , _isScanning(false)
00881 //     , _isSupportingMotorCtrl(false)
00882 // {
00883 //     _cached_scan_node_hq_count = 0;
00884 //     _cached_scan_node_hq_count_for_interval_retrieve = 0;
00885 //     _cached_sampleduration_std = LEGACY_SAMPLE_DURATION;
00886 //     _cached_sampleduration_express = LEGACY_SAMPLE_DURATION;
00887 // }
00888