Ben Gordon / Mbed 2 deprecated rplidar_arduino-master

lidar code for ROC318

Dependencies:   BufferedSerial mbed

rplidar/RPlidar.cpp@0:0791d48ee421, 2018-10-19 (annotated)

Committer:
BenRJG
Date:
Fri Oct 19 17:42:05 2018 +0000
Revision:
0:0791d48ee421
Imported lidar code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BenRJG 0:0791d48ee421 1 /*
BenRJG 0:0791d48ee421 2 * RoboPeak RPLIDAR Driver for Arduino
BenRJG 0:0791d48ee421 3 * RoboPeak.com
BenRJG 0:0791d48ee421 4 *
BenRJG 0:0791d48ee421 5 * Copyright (c) 2014, RoboPeak
BenRJG 0:0791d48ee421 6 * All rights reserved.
BenRJG 0:0791d48ee421 7 *
BenRJG 0:0791d48ee421 8 * Redistribution and use in source and binary forms, with or without modification,
BenRJG 0:0791d48ee421 9 * are permitted provided that the following conditions are met:
BenRJG 0:0791d48ee421 10 *
BenRJG 0:0791d48ee421 11 * 1. Redistributions of source code must retain the above copyright notice,
BenRJG 0:0791d48ee421 12 * this list of conditions and the following disclaimer.
BenRJG 0:0791d48ee421 13 *
BenRJG 0:0791d48ee421 14 * 2. Redistributions in binary form must reproduce the above copyright notice,
BenRJG 0:0791d48ee421 15 * this list of conditions and the following disclaimer in the documentation
BenRJG 0:0791d48ee421 16 * and/or other materials provided with the distribution.
BenRJG 0:0791d48ee421 17 *
BenRJG 0:0791d48ee421 18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
BenRJG 0:0791d48ee421 19 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
BenRJG 0:0791d48ee421 20 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
BenRJG 0:0791d48ee421 21 * SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
BenRJG 0:0791d48ee421 22 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
BenRJG 0:0791d48ee421 23 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
BenRJG 0:0791d48ee421 24 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
BenRJG 0:0791d48ee421 25 * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
BenRJG 0:0791d48ee421 26 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
BenRJG 0:0791d48ee421 27 *
BenRJG 0:0791d48ee421 28 */
BenRJG 0:0791d48ee421 29
BenRJG 0:0791d48ee421 30 #include "rplidar.h"
BenRJG 0:0791d48ee421 31 //BufferedSerial _bined_serialdev( PA_11, PA_12); // tx, rx
BenRJG 0:0791d48ee421 32 Timer timers;
BenRJG 0:0791d48ee421 33 RPLidar::RPLidar()
BenRJG 0:0791d48ee421 34 {
BenRJG 0:0791d48ee421 35
BenRJG 0:0791d48ee421 36 _currentMeasurement.distance = 0;
BenRJG 0:0791d48ee421 37 _currentMeasurement.angle = 0;
BenRJG 0:0791d48ee421 38 _currentMeasurement.quality = 0;
BenRJG 0:0791d48ee421 39 _currentMeasurement.startBit = 0;
BenRJG 0:0791d48ee421 40 }
BenRJG 0:0791d48ee421 41
BenRJG 0:0791d48ee421 42
BenRJG 0:0791d48ee421 43 RPLidar::~RPLidar()
BenRJG 0:0791d48ee421 44 {
BenRJG 0:0791d48ee421 45 end();
BenRJG 0:0791d48ee421 46 }
BenRJG 0:0791d48ee421 47
BenRJG 0:0791d48ee421 48 // open the given serial interface and try to connect to the RPLIDAR
BenRJG 0:0791d48ee421 49 /*
BenRJG 0:0791d48ee421 50 bool RPLidar::begin(BufferedSerial &serialobj)
BenRJG 0:0791d48ee421 51 {
BenRJG 0:0791d48ee421 52
BenRJG 0:0791d48ee421 53 //Serial.begin(115200);
BenRJG 0:0791d48ee421 54
BenRJG 0:0791d48ee421 55 if (isOpen()) {
BenRJG 0:0791d48ee421 56 end();
BenRJG 0:0791d48ee421 57 }
BenRJG 0:0791d48ee421 58 _bined_serialdev = &serialobj;
BenRJG 0:0791d48ee421 59 // _bined_serialdev->end();
BenRJG 0:0791d48ee421 60 _bined_serialdev->baud(RPLIDAR_SERIAL_BAUDRATE);
BenRJG 0:0791d48ee421 61 }
BenRJG 0:0791d48ee421 62 */
BenRJG 0:0791d48ee421 63 void RPLidar::begin(BufferedSerial &serialobj)
BenRJG 0:0791d48ee421 64 {
BenRJG 0:0791d48ee421 65 _bined_serialdev = &serialobj;
BenRJG 0:0791d48ee421 66 timers.start();
BenRJG 0:0791d48ee421 67 _bined_serialdev->baud(RPLIDAR_SERIAL_BAUDRATE);
BenRJG 0:0791d48ee421 68 }
BenRJG 0:0791d48ee421 69 // close the currently opened serial interface
BenRJG 0:0791d48ee421 70 void RPLidar::end()
BenRJG 0:0791d48ee421 71 {/*
BenRJG 0:0791d48ee421 72 if (isOpen()) {
BenRJG 0:0791d48ee421 73 _bined_serialdev->end();
BenRJG 0:0791d48ee421 74 _bined_serialdev = NULL;
BenRJG 0:0791d48ee421 75 }*/
BenRJG 0:0791d48ee421 76 }
BenRJG 0:0791d48ee421 77
BenRJG 0:0791d48ee421 78
BenRJG 0:0791d48ee421 79 // check whether the serial interface is opened
BenRJG 0:0791d48ee421 80 /*
BenRJG 0:0791d48ee421 81 bool RPLidar::isOpen()
BenRJG 0:0791d48ee421 82 {
BenRJG 0:0791d48ee421 83 return _bined_serialdev?true:false;
BenRJG 0:0791d48ee421 84 }
BenRJG 0:0791d48ee421 85 */
BenRJG 0:0791d48ee421 86 // ask the RPLIDAR for its health info
BenRJG 0:0791d48ee421 87
BenRJG 0:0791d48ee421 88 u_result RPLidar::getHealth(rplidar_response_device_health_t & healthinfo, _u32 timeout)
BenRJG 0:0791d48ee421 89 {
BenRJG 0:0791d48ee421 90 _u32 currentTs = timers.read_ms();
BenRJG 0:0791d48ee421 91 _u32 remainingtime;
BenRJG 0:0791d48ee421 92
BenRJG 0:0791d48ee421 93 _u8 *infobuf = (_u8 *)&healthinfo;
BenRJG 0:0791d48ee421 94 _u8 recvPos = 0;
BenRJG 0:0791d48ee421 95
BenRJG 0:0791d48ee421 96 rplidar_ans_header_t response_header;
BenRJG 0:0791d48ee421 97 u_result ans;
BenRJG 0:0791d48ee421 98
BenRJG 0:0791d48ee421 99
BenRJG 0:0791d48ee421 100 // if (!isOpen()) return RESULT_OPERATION_FAIL;
BenRJG 0:0791d48ee421 101
BenRJG 0:0791d48ee421 102 {
BenRJG 0:0791d48ee421 103 if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_HEALTH, NULL, 0))) {
BenRJG 0:0791d48ee421 104 return ans;
BenRJG 0:0791d48ee421 105 }
BenRJG 0:0791d48ee421 106
BenRJG 0:0791d48ee421 107 if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
BenRJG 0:0791d48ee421 108 return ans;
BenRJG 0:0791d48ee421 109 }
BenRJG 0:0791d48ee421 110
BenRJG 0:0791d48ee421 111 // verify whether we got a correct header
BenRJG 0:0791d48ee421 112 if (response_header.type != RPLIDAR_ANS_TYPE_DEVHEALTH) {
BenRJG 0:0791d48ee421 113 return RESULT_INVALID_DATA;
BenRJG 0:0791d48ee421 114 }
BenRJG 0:0791d48ee421 115
BenRJG 0:0791d48ee421 116 if ((response_header.size) < sizeof(rplidar_response_device_health_t)) {
BenRJG 0:0791d48ee421 117 return RESULT_INVALID_DATA;
BenRJG 0:0791d48ee421 118 }
BenRJG 0:0791d48ee421 119
BenRJG 0:0791d48ee421 120 while ((remainingtime=timers.read_ms() - currentTs) <= timeout) {
BenRJG 0:0791d48ee421 121 int currentbyte = _bined_serialdev->getc();
BenRJG 0:0791d48ee421 122 if (currentbyte < 0) continue;
BenRJG 0:0791d48ee421 123
BenRJG 0:0791d48ee421 124 infobuf[recvPos++] = currentbyte;
BenRJG 0:0791d48ee421 125
BenRJG 0:0791d48ee421 126 if (recvPos == sizeof(rplidar_response_device_health_t)) {
BenRJG 0:0791d48ee421 127 return RESULT_OK;
BenRJG 0:0791d48ee421 128 }
BenRJG 0:0791d48ee421 129 }
BenRJG 0:0791d48ee421 130 }
BenRJG 0:0791d48ee421 131 return RESULT_OPERATION_TIMEOUT;
BenRJG 0:0791d48ee421 132 }
BenRJG 0:0791d48ee421 133 // ask the RPLIDAR for its device info like the serial number
BenRJG 0:0791d48ee421 134 u_result RPLidar::getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout )
BenRJG 0:0791d48ee421 135 {
BenRJG 0:0791d48ee421 136 _u8 recvPos = 0;
BenRJG 0:0791d48ee421 137 _u32 currentTs = timers.read_ms();
BenRJG 0:0791d48ee421 138 _u32 remainingtime;
BenRJG 0:0791d48ee421 139 _u8 *infobuf = (_u8*)&info;
BenRJG 0:0791d48ee421 140 rplidar_ans_header_t response_header;
BenRJG 0:0791d48ee421 141 u_result ans;
BenRJG 0:0791d48ee421 142
BenRJG 0:0791d48ee421 143 // if (!isOpen()) return RESULT_OPERATION_FAIL;
BenRJG 0:0791d48ee421 144
BenRJG 0:0791d48ee421 145 {
BenRJG 0:0791d48ee421 146 if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_INFO,NULL,0))) {
BenRJG 0:0791d48ee421 147 return ans;
BenRJG 0:0791d48ee421 148 }
BenRJG 0:0791d48ee421 149
BenRJG 0:0791d48ee421 150 if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
BenRJG 0:0791d48ee421 151 return ans;
BenRJG 0:0791d48ee421 152 }
BenRJG 0:0791d48ee421 153
BenRJG 0:0791d48ee421 154 // verify whether we got a correct header
BenRJG 0:0791d48ee421 155 if (response_header.type != RPLIDAR_ANS_TYPE_DEVINFO) {
BenRJG 0:0791d48ee421 156 return RESULT_INVALID_DATA;
BenRJG 0:0791d48ee421 157 }
BenRJG 0:0791d48ee421 158
BenRJG 0:0791d48ee421 159 if (response_header.size < sizeof(rplidar_response_device_info_t)) {
BenRJG 0:0791d48ee421 160 return RESULT_INVALID_DATA;
BenRJG 0:0791d48ee421 161 }
BenRJG 0:0791d48ee421 162
BenRJG 0:0791d48ee421 163 while ((remainingtime=timers.read_ms() - currentTs) <= timeout) {
BenRJG 0:0791d48ee421 164 int currentbyte = _bined_serialdev->getc();
BenRJG 0:0791d48ee421 165 if (currentbyte<0) continue;
BenRJG 0:0791d48ee421 166 infobuf[recvPos++] = currentbyte;
BenRJG 0:0791d48ee421 167
BenRJG 0:0791d48ee421 168 if (recvPos == sizeof(rplidar_response_device_info_t)) {
BenRJG 0:0791d48ee421 169 return RESULT_OK;
BenRJG 0:0791d48ee421 170 }
BenRJG 0:0791d48ee421 171 }
BenRJG 0:0791d48ee421 172 }
BenRJG 0:0791d48ee421 173
BenRJG 0:0791d48ee421 174 return RESULT_OPERATION_TIMEOUT;
BenRJG 0:0791d48ee421 175 }
BenRJG 0:0791d48ee421 176
BenRJG 0:0791d48ee421 177 // stop the measurement operation
BenRJG 0:0791d48ee421 178 u_result RPLidar::stop()
BenRJG 0:0791d48ee421 179 {
BenRJG 0:0791d48ee421 180 // if (!isOpen()) return RESULT_OPERATION_FAIL;
BenRJG 0:0791d48ee421 181 u_result ans = _sendCommand(RPLIDAR_CMD_STOP,NULL,0);
BenRJG 0:0791d48ee421 182 return ans;
BenRJG 0:0791d48ee421 183 }
BenRJG 0:0791d48ee421 184
BenRJG 0:0791d48ee421 185 // start the measurement operation
BenRJG 0:0791d48ee421 186 u_result RPLidar::startScan(bool force, _u32 timeout)
BenRJG 0:0791d48ee421 187 {
BenRJG 0:0791d48ee421 188 u_result ans;
BenRJG 0:0791d48ee421 189
BenRJG 0:0791d48ee421 190 // if (!isOpen()) return RESULT_OPERATION_FAIL;
BenRJG 0:0791d48ee421 191
BenRJG 0:0791d48ee421 192 stop(); //force the previous operation to stop
BenRJG 0:0791d48ee421 193
BenRJG 0:0791d48ee421 194
BenRJG 0:0791d48ee421 195 ans = _sendCommand(force?RPLIDAR_CMD_FORCE_SCAN:RPLIDAR_CMD_SCAN, NULL, 0);
BenRJG 0:0791d48ee421 196 if (IS_FAIL(ans)) return ans;
BenRJG 0:0791d48ee421 197
BenRJG 0:0791d48ee421 198 // waiting for confirmation
BenRJG 0:0791d48ee421 199 rplidar_ans_header_t response_header;
BenRJG 0:0791d48ee421 200 if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
BenRJG 0:0791d48ee421 201 return ans;
BenRJG 0:0791d48ee421 202 }
BenRJG 0:0791d48ee421 203
BenRJG 0:0791d48ee421 204 // verify whether we got a correct header
BenRJG 0:0791d48ee421 205 if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT) {
BenRJG 0:0791d48ee421 206 return RESULT_INVALID_DATA;
BenRJG 0:0791d48ee421 207 }
BenRJG 0:0791d48ee421 208
BenRJG 0:0791d48ee421 209 if (response_header.size < sizeof(rplidar_response_measurement_node_t)) {
BenRJG 0:0791d48ee421 210 return RESULT_INVALID_DATA;
BenRJG 0:0791d48ee421 211 }
BenRJG 0:0791d48ee421 212
BenRJG 0:0791d48ee421 213 return RESULT_OK;
BenRJG 0:0791d48ee421 214 }
BenRJG 0:0791d48ee421 215
BenRJG 0:0791d48ee421 216 // wait for one sample point to arrive
BenRJG 0:0791d48ee421 217 u_result RPLidar::waitPoint(_u32 timeout)
BenRJG 0:0791d48ee421 218 {
BenRJG 0:0791d48ee421 219 _u32 currentTs = timers.read_ms();
BenRJG 0:0791d48ee421 220 _u32 remainingtime;
BenRJG 0:0791d48ee421 221 rplidar_response_measurement_node_t node;
BenRJG 0:0791d48ee421 222 _u8 *nodebuf = (_u8*)&node;
BenRJG 0:0791d48ee421 223
BenRJG 0:0791d48ee421 224 _u8 recvPos = 0;
BenRJG 0:0791d48ee421 225
BenRJG 0:0791d48ee421 226 while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
BenRJG 0:0791d48ee421 227 int currentbyte = _bined_serialdev->getc();
BenRJG 0:0791d48ee421 228 if (currentbyte<0) continue;
BenRJG 0:0791d48ee421 229 //Serial.println(currentbyte);
BenRJG 0:0791d48ee421 230 switch (recvPos) {
BenRJG 0:0791d48ee421 231 case 0: // expect the sync bit and its reverse in this byte {
BenRJG 0:0791d48ee421 232 {
BenRJG 0:0791d48ee421 233 _u8 tmp = (currentbyte>>1);
BenRJG 0:0791d48ee421 234 if ( (tmp ^ currentbyte) & 0x1 ) {
BenRJG 0:0791d48ee421 235 // pass
BenRJG 0:0791d48ee421 236 } else {
BenRJG 0:0791d48ee421 237 continue;
BenRJG 0:0791d48ee421 238 }
BenRJG 0:0791d48ee421 239
BenRJG 0:0791d48ee421 240 }
BenRJG 0:0791d48ee421 241 break;
BenRJG 0:0791d48ee421 242 case 1: // expect the highest bit to be 1
BenRJG 0:0791d48ee421 243 {
BenRJG 0:0791d48ee421 244 if (currentbyte & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) {
BenRJG 0:0791d48ee421 245 // pass
BenRJG 0:0791d48ee421 246 } else {
BenRJG 0:0791d48ee421 247 recvPos = 0;
BenRJG 0:0791d48ee421 248 continue;
BenRJG 0:0791d48ee421 249 }
BenRJG 0:0791d48ee421 250 }
BenRJG 0:0791d48ee421 251 break;
BenRJG 0:0791d48ee421 252 }
BenRJG 0:0791d48ee421 253 nodebuf[recvPos++] = currentbyte;
BenRJG 0:0791d48ee421 254 if (recvPos == sizeof(rplidar_response_measurement_node_t)) {
BenRJG 0:0791d48ee421 255 // store the data ...
BenRJG 0:0791d48ee421 256 _currentMeasurement.distance = node.distance_q2/4.0f;
BenRJG 0:0791d48ee421 257 _currentMeasurement.angle = (node.angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
BenRJG 0:0791d48ee421 258 _currentMeasurement.quality = (node.sync_quality>>RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
BenRJG 0:0791d48ee421 259 _currentMeasurement.startBit = (node.sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT);
BenRJG 0:0791d48ee421 260 ang = _currentMeasurement.angle;
BenRJG 0:0791d48ee421 261 dis = _currentMeasurement.distance;
BenRJG 0:0791d48ee421 262
BenRJG 0:0791d48ee421 263
BenRJG 0:0791d48ee421 264 if(ang>=angMin&&ang<=angMax)Data[ang] = dis;
BenRJG 0:0791d48ee421 265
BenRJG 0:0791d48ee421 266 return RESULT_OK;
BenRJG 0:0791d48ee421 267 }
BenRJG 0:0791d48ee421 268
BenRJG 0:0791d48ee421 269
BenRJG 0:0791d48ee421 270 }
BenRJG 0:0791d48ee421 271
BenRJG 0:0791d48ee421 272 return RESULT_OPERATION_TIMEOUT;
BenRJG 0:0791d48ee421 273 }
BenRJG 0:0791d48ee421 274
BenRJG 0:0791d48ee421 275
BenRJG 0:0791d48ee421 276 void RPLidar::setAngle(int min,int max){
BenRJG 0:0791d48ee421 277 angMin = min;
BenRJG 0:0791d48ee421 278 angMax = max;
BenRJG 0:0791d48ee421 279 }
BenRJG 0:0791d48ee421 280 void RPLidar::get_lidar(){
BenRJG 0:0791d48ee421 281 if (!IS_OK(waitPoint())) startScan();
BenRJG 0:0791d48ee421 282 }
BenRJG 0:0791d48ee421 283 u_result RPLidar::_sendCommand(_u8 cmd, const void * payload, size_t payloadsize)
BenRJG 0:0791d48ee421 284 {
BenRJG 0:0791d48ee421 285
BenRJG 0:0791d48ee421 286 rplidar_cmd_packet_t pkt_header;
BenRJG 0:0791d48ee421 287 rplidar_cmd_packet_t * header = &pkt_header;
BenRJG 0:0791d48ee421 288 _u8 checksum = 0;
BenRJG 0:0791d48ee421 289
BenRJG 0:0791d48ee421 290 if (payloadsize && payload) {
BenRJG 0:0791d48ee421 291 cmd |= RPLIDAR_CMDFLAG_HAS_PAYLOAD;
BenRJG 0:0791d48ee421 292 }
BenRJG 0:0791d48ee421 293
BenRJG 0:0791d48ee421 294 header->syncByte = RPLIDAR_CMD_SYNC_BYTE;
BenRJG 0:0791d48ee421 295 header->cmd_flag = cmd;
BenRJG 0:0791d48ee421 296
BenRJG 0:0791d48ee421 297 // send header first
BenRJG 0:0791d48ee421 298 _bined_serialdev->putc(header->syncByte );
BenRJG 0:0791d48ee421 299 _bined_serialdev->putc(header->cmd_flag );
BenRJG 0:0791d48ee421 300
BenRJG 0:0791d48ee421 301 // _bined_serialdev->write( (uint8_t *)header, 2);
BenRJG 0:0791d48ee421 302
BenRJG 0:0791d48ee421 303 if (cmd & RPLIDAR_CMDFLAG_HAS_PAYLOAD) {
BenRJG 0:0791d48ee421 304 checksum ^= RPLIDAR_CMD_SYNC_BYTE;
BenRJG 0:0791d48ee421 305 checksum ^= cmd;
BenRJG 0:0791d48ee421 306 checksum ^= (payloadsize & 0xFF);
BenRJG 0:0791d48ee421 307
BenRJG 0:0791d48ee421 308 // calc checksum
BenRJG 0:0791d48ee421 309 for (size_t pos = 0; pos < payloadsize; ++pos) {
BenRJG 0:0791d48ee421 310 checksum ^= ((_u8 *)payload)[pos];
BenRJG 0:0791d48ee421 311 }
BenRJG 0:0791d48ee421 312
BenRJG 0:0791d48ee421 313 // send size
BenRJG 0:0791d48ee421 314 _u8 sizebyte = payloadsize;
BenRJG 0:0791d48ee421 315 _bined_serialdev->putc(sizebyte);
BenRJG 0:0791d48ee421 316 // _bined_serialdev->write((uint8_t *)&sizebyte, 1);
BenRJG 0:0791d48ee421 317
BenRJG 0:0791d48ee421 318 // send payload
BenRJG 0:0791d48ee421 319 // _bined_serialdev.putc((uint8_t )payload);
BenRJG 0:0791d48ee421 320 // _bined_serialdev->write((uint8_t *)&payload, sizebyte);
BenRJG 0:0791d48ee421 321
BenRJG 0:0791d48ee421 322 // send checksum
BenRJG 0:0791d48ee421 323 _bined_serialdev->putc(checksum);
BenRJG 0:0791d48ee421 324 // _bined_serialdev->write((uint8_t *)&checksum, 1);
BenRJG 0:0791d48ee421 325
BenRJG 0:0791d48ee421 326 }
BenRJG 0:0791d48ee421 327
BenRJG 0:0791d48ee421 328 return RESULT_OK;
BenRJG 0:0791d48ee421 329 }
BenRJG 0:0791d48ee421 330
BenRJG 0:0791d48ee421 331 u_result RPLidar::_waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout)
BenRJG 0:0791d48ee421 332 {
BenRJG 0:0791d48ee421 333 _u8 recvPos = 0;
BenRJG 0:0791d48ee421 334 _u32 currentTs = timers.read_ms();
BenRJG 0:0791d48ee421 335 _u32 remainingtime;
BenRJG 0:0791d48ee421 336 _u8 *headerbuf = (_u8*)header;
BenRJG 0:0791d48ee421 337 while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
BenRJG 0:0791d48ee421 338
BenRJG 0:0791d48ee421 339 int currentbyte = _bined_serialdev->getc();
BenRJG 0:0791d48ee421 340 if (currentbyte<0) continue;
BenRJG 0:0791d48ee421 341 switch (recvPos) {
BenRJG 0:0791d48ee421 342 case 0:
BenRJG 0:0791d48ee421 343 if (currentbyte != RPLIDAR_ANS_SYNC_BYTE1) {
BenRJG 0:0791d48ee421 344 continue;
BenRJG 0:0791d48ee421 345 }
BenRJG 0:0791d48ee421 346 break;
BenRJG 0:0791d48ee421 347 case 1:
BenRJG 0:0791d48ee421 348 if (currentbyte != RPLIDAR_ANS_SYNC_BYTE2) {
BenRJG 0:0791d48ee421 349 recvPos = 0;
BenRJG 0:0791d48ee421 350 continue;
BenRJG 0:0791d48ee421 351 }
BenRJG 0:0791d48ee421 352 break;
BenRJG 0:0791d48ee421 353 }
BenRJG 0:0791d48ee421 354 headerbuf[recvPos++] = currentbyte;
BenRJG 0:0791d48ee421 355
BenRJG 0:0791d48ee421 356 if (recvPos == sizeof(rplidar_ans_header_t)) {
BenRJG 0:0791d48ee421 357 return RESULT_OK;
BenRJG 0:0791d48ee421 358 }
BenRJG 0:0791d48ee421 359
BenRJG 0:0791d48ee421 360
BenRJG 0:0791d48ee421 361 }
BenRJG 0:0791d48ee421 362
BenRJG 0:0791d48ee421 363 return RESULT_OPERATION_TIMEOUT;
BenRJG 0:0791d48ee421 364 }