Arsapol Manamunchaiyaporn / Mbed 2 deprecated CleaningM-Palm

before test

Dependencies:   BEAR_Protocol_Edited BufferedSerial Debug MaxSonar PID Process QEI UI iSerial mbed

Fork of clean_V1 by Betago

rplidar/RPlidar.cpp@7:0dac9d4ff04f, 2016-06-07 (annotated)

Committer:
icyzkungz
Date:
Tue Jun 07 16:17:34 2016 +0000
Revision:
7:0dac9d4ff04f
Parent:
5:fe76f3dae81e 
before test

Who changed what in which revision?

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