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@4:de5a65c17664, 2016-06-05 (annotated)

Committer:
palmdotax
Date:
Sun Jun 05 09:43:40 2016 +0000
Revision:
4:de5a65c17664
Parent:
2:f873deba2305
Child:
5:fe76f3dae81e
v1;

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 2:f873deba2305 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 2:f873deba2305 63 void RPLidar::begin()
palmdotax 2:f873deba2305 64 {
palmdotax 2:f873deba2305 65 //BufferedSerial lidar_serial(PinName PA_11, PinName PA_12);
palmdotax 2:f873deba2305 66 //Serial.begin(115200);
palmdotax 2:f873deba2305 67 timers.start();
palmdotax 2:f873deba2305 68 _bined_serialdev.baud(115200);
palmdotax 2:f873deba2305 69 }
palmdotax 2:f873deba2305 70 // close the currently opened serial interface
palmdotax 2:f873deba2305 71 void RPLidar::end()
palmdotax 2:f873deba2305 72 {/*
palmdotax 2:f873deba2305 73 if (isOpen()) {
palmdotax 2:f873deba2305 74 _bined_serialdev->end();
palmdotax 2:f873deba2305 75 _bined_serialdev = NULL;
palmdotax 2:f873deba2305 76 }*/
palmdotax 2:f873deba2305 77 }
palmdotax 2:f873deba2305 78
palmdotax 2:f873deba2305 79
palmdotax 2:f873deba2305 80 // check whether the serial interface is opened
palmdotax 2:f873deba2305 81 /*
palmdotax 2:f873deba2305 82 bool RPLidar::isOpen()
palmdotax 2:f873deba2305 83 {
palmdotax 2:f873deba2305 84 return _bined_serialdev?true:false;
palmdotax 2:f873deba2305 85 }
palmdotax 2:f873deba2305 86 */
palmdotax 2:f873deba2305 87 // ask the RPLIDAR for its health info
palmdotax 2:f873deba2305 88
palmdotax 2:f873deba2305 89 u_result RPLidar::getHealth(rplidar_response_device_health_t & healthinfo, _u32 timeout)
palmdotax 2:f873deba2305 90 {
palmdotax 2:f873deba2305 91 _u32 currentTs = timers.read_ms();
palmdotax 2:f873deba2305 92 _u32 remainingtime;
palmdotax 2:f873deba2305 93
palmdotax 2:f873deba2305 94 _u8 *infobuf = (_u8 *)&healthinfo;
palmdotax 2:f873deba2305 95 _u8 recvPos = 0;
palmdotax 2:f873deba2305 96
palmdotax 2:f873deba2305 97 rplidar_ans_header_t response_header;
palmdotax 2:f873deba2305 98 u_result ans;
palmdotax 2:f873deba2305 99
palmdotax 2:f873deba2305 100
palmdotax 2:f873deba2305 101 // if (!isOpen()) return RESULT_OPERATION_FAIL;
palmdotax 2:f873deba2305 102
palmdotax 2:f873deba2305 103 {
palmdotax 2:f873deba2305 104 if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_HEALTH, NULL, 0))) {
palmdotax 2:f873deba2305 105 return ans;
palmdotax 2:f873deba2305 106 }
palmdotax 2:f873deba2305 107
palmdotax 2:f873deba2305 108 if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
palmdotax 2:f873deba2305 109 return ans;
palmdotax 2:f873deba2305 110 }
palmdotax 2:f873deba2305 111
palmdotax 2:f873deba2305 112 // verify whether we got a correct header
palmdotax 2:f873deba2305 113 if (response_header.type != RPLIDAR_ANS_TYPE_DEVHEALTH) {
palmdotax 2:f873deba2305 114 return RESULT_INVALID_DATA;
palmdotax 2:f873deba2305 115 }
palmdotax 2:f873deba2305 116
palmdotax 2:f873deba2305 117 if ((response_header.size) < sizeof(rplidar_response_device_health_t)) {
palmdotax 2:f873deba2305 118 return RESULT_INVALID_DATA;
palmdotax 2:f873deba2305 119 }
palmdotax 2:f873deba2305 120
palmdotax 2:f873deba2305 121 while ((remainingtime=timers.read_ms() - currentTs) <= timeout) {
palmdotax 2:f873deba2305 122 int currentbyte = _bined_serialdev.getc();
palmdotax 2:f873deba2305 123 if (currentbyte < 0) continue;
palmdotax 2:f873deba2305 124
palmdotax 2:f873deba2305 125 infobuf[recvPos++] = currentbyte;
palmdotax 2:f873deba2305 126
palmdotax 2:f873deba2305 127 if (recvPos == sizeof(rplidar_response_device_health_t)) {
palmdotax 2:f873deba2305 128 return RESULT_OK;
palmdotax 2:f873deba2305 129 }
palmdotax 2:f873deba2305 130 }
palmdotax 2:f873deba2305 131 }
palmdotax 2:f873deba2305 132 return RESULT_OPERATION_TIMEOUT;
palmdotax 2:f873deba2305 133 }
palmdotax 2:f873deba2305 134 // ask the RPLIDAR for its device info like the serial number
palmdotax 2:f873deba2305 135 u_result RPLidar::getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout )
palmdotax 2:f873deba2305 136 {
palmdotax 2:f873deba2305 137 _u8 recvPos = 0;
palmdotax 2:f873deba2305 138 _u32 currentTs = timers.read_ms();
palmdotax 2:f873deba2305 139 _u32 remainingtime;
palmdotax 2:f873deba2305 140 _u8 *infobuf = (_u8*)&info;
palmdotax 2:f873deba2305 141 rplidar_ans_header_t response_header;
palmdotax 2:f873deba2305 142 u_result ans;
palmdotax 2:f873deba2305 143
palmdotax 2:f873deba2305 144 // if (!isOpen()) return RESULT_OPERATION_FAIL;
palmdotax 2:f873deba2305 145
palmdotax 2:f873deba2305 146 {
palmdotax 2:f873deba2305 147 if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_INFO,NULL,0))) {
palmdotax 2:f873deba2305 148 return ans;
palmdotax 2:f873deba2305 149 }
palmdotax 2:f873deba2305 150
palmdotax 2:f873deba2305 151 if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
palmdotax 2:f873deba2305 152 return ans;
palmdotax 2:f873deba2305 153 }
palmdotax 2:f873deba2305 154
palmdotax 2:f873deba2305 155 // verify whether we got a correct header
palmdotax 2:f873deba2305 156 if (response_header.type != RPLIDAR_ANS_TYPE_DEVINFO) {
palmdotax 2:f873deba2305 157 return RESULT_INVALID_DATA;
palmdotax 2:f873deba2305 158 }
palmdotax 2:f873deba2305 159
palmdotax 2:f873deba2305 160 if (response_header.size < sizeof(rplidar_response_device_info_t)) {
palmdotax 2:f873deba2305 161 return RESULT_INVALID_DATA;
palmdotax 2:f873deba2305 162 }
palmdotax 2:f873deba2305 163
palmdotax 2:f873deba2305 164 while ((remainingtime=timers.read_ms() - currentTs) <= timeout) {
palmdotax 2:f873deba2305 165 int currentbyte = _bined_serialdev.getc();
palmdotax 2:f873deba2305 166 if (currentbyte<0) continue;
palmdotax 2:f873deba2305 167 infobuf[recvPos++] = currentbyte;
palmdotax 2:f873deba2305 168
palmdotax 2:f873deba2305 169 if (recvPos == sizeof(rplidar_response_device_info_t)) {
palmdotax 2:f873deba2305 170 return RESULT_OK;
palmdotax 2:f873deba2305 171 }
palmdotax 2:f873deba2305 172 }
palmdotax 2:f873deba2305 173 }
palmdotax 2:f873deba2305 174
palmdotax 2:f873deba2305 175 return RESULT_OPERATION_TIMEOUT;
palmdotax 2:f873deba2305 176 }
palmdotax 2:f873deba2305 177
palmdotax 2:f873deba2305 178 // stop the measurement operation
palmdotax 2:f873deba2305 179 u_result RPLidar::stop()
palmdotax 2:f873deba2305 180 {
palmdotax 2:f873deba2305 181 // if (!isOpen()) return RESULT_OPERATION_FAIL;
palmdotax 2:f873deba2305 182 u_result ans = _sendCommand(RPLIDAR_CMD_STOP,NULL,0);
palmdotax 2:f873deba2305 183 return ans;
palmdotax 2:f873deba2305 184 }
palmdotax 2:f873deba2305 185
palmdotax 2:f873deba2305 186 // start the measurement operation
palmdotax 2:f873deba2305 187 u_result RPLidar::startScan(bool force, _u32 timeout)
palmdotax 2:f873deba2305 188 {
palmdotax 2:f873deba2305 189 /*
palmdotax 2:f873deba2305 190 rplidar_cmd_packet_t pkt_header;
palmdotax 2:f873deba2305 191 rplidar_cmd_packet_t * header = &pkt_header;
palmdotax 2:f873deba2305 192 header->syncByte = 0xA5;
palmdotax 2:f873deba2305 193 header->cmd_flag = 0x21;
palmdotax 2:f873deba2305 194 //se.write(12);
palmdotax 2:f873deba2305 195
palmdotax 2:f873deba2305 196 _bined_serialdev.putc(header->syncByte );
palmdotax 2:f873deba2305 197 _bined_serialdev.putc(header->cmd_flag );
palmdotax 2:f873deba2305 198 */
palmdotax 2:f873deba2305 199
palmdotax 2:f873deba2305 200 u_result ans;
palmdotax 2:f873deba2305 201
palmdotax 2:f873deba2305 202 // if (!isOpen()) return RESULT_OPERATION_FAIL;
palmdotax 2:f873deba2305 203
palmdotax 2:f873deba2305 204 stop(); //force the previous operation to stop
palmdotax 2:f873deba2305 205
palmdotax 2:f873deba2305 206
palmdotax 2:f873deba2305 207 ans = _sendCommand(force?RPLIDAR_CMD_FORCE_SCAN:RPLIDAR_CMD_SCAN, NULL, 0);
palmdotax 2:f873deba2305 208 if (IS_FAIL(ans)) return ans;
palmdotax 2:f873deba2305 209
palmdotax 2:f873deba2305 210 // waiting for confirmation
palmdotax 2:f873deba2305 211 rplidar_ans_header_t response_header;
palmdotax 2:f873deba2305 212 if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
palmdotax 2:f873deba2305 213 return ans;
palmdotax 2:f873deba2305 214 }
palmdotax 2:f873deba2305 215
palmdotax 2:f873deba2305 216 // verify whether we got a correct header
palmdotax 2:f873deba2305 217 if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT) {
palmdotax 2:f873deba2305 218 return RESULT_INVALID_DATA;
palmdotax 2:f873deba2305 219 }
palmdotax 2:f873deba2305 220
palmdotax 2:f873deba2305 221 if (response_header.size < sizeof(rplidar_response_measurement_node_t)) {
palmdotax 2:f873deba2305 222 return RESULT_INVALID_DATA;
palmdotax 2:f873deba2305 223 }
palmdotax 2:f873deba2305 224
palmdotax 2:f873deba2305 225 return RESULT_OK;
palmdotax 2:f873deba2305 226 }
palmdotax 2:f873deba2305 227
palmdotax 2:f873deba2305 228 // wait for one sample point to arrive
palmdotax 2:f873deba2305 229 u_result RPLidar::waitPoint(_u32 timeout)
palmdotax 2:f873deba2305 230 {
palmdotax 2:f873deba2305 231 _u32 currentTs = timers.read_ms();
palmdotax 2:f873deba2305 232 _u32 remainingtime;
palmdotax 2:f873deba2305 233 rplidar_response_measurement_node_t node;
palmdotax 2:f873deba2305 234 _u8 *nodebuf = (_u8*)&node;
palmdotax 2:f873deba2305 235
palmdotax 2:f873deba2305 236 _u8 recvPos = 0;
palmdotax 2:f873deba2305 237
palmdotax 2:f873deba2305 238 while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
palmdotax 2:f873deba2305 239 int currentbyte = _bined_serialdev.getc();
palmdotax 2:f873deba2305 240 if (currentbyte<0) continue;
palmdotax 2:f873deba2305 241 //Serial.println(currentbyte);
palmdotax 2:f873deba2305 242 switch (recvPos) {
palmdotax 2:f873deba2305 243 case 0: // expect the sync bit and its reverse in this byte {
palmdotax 2:f873deba2305 244 {
palmdotax 2:f873deba2305 245 _u8 tmp = (currentbyte>>1);
palmdotax 2:f873deba2305 246 if ( (tmp ^ currentbyte) & 0x1 ) {
palmdotax 2:f873deba2305 247 // pass
palmdotax 2:f873deba2305 248 } else {
palmdotax 2:f873deba2305 249 continue;
palmdotax 2:f873deba2305 250 }
palmdotax 2:f873deba2305 251
palmdotax 2:f873deba2305 252 }
palmdotax 2:f873deba2305 253 break;
palmdotax 2:f873deba2305 254 case 1: // expect the highest bit to be 1
palmdotax 2:f873deba2305 255 {
palmdotax 2:f873deba2305 256 if (currentbyte & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) {
palmdotax 2:f873deba2305 257 // pass
palmdotax 2:f873deba2305 258 } else {
palmdotax 2:f873deba2305 259 recvPos = 0;
palmdotax 2:f873deba2305 260 continue;
palmdotax 2:f873deba2305 261 }
palmdotax 2:f873deba2305 262 }
palmdotax 2:f873deba2305 263 break;
palmdotax 2:f873deba2305 264 }
palmdotax 2:f873deba2305 265 nodebuf[recvPos++] = currentbyte;
palmdotax 2:f873deba2305 266 if (recvPos == sizeof(rplidar_response_measurement_node_t)) {
palmdotax 2:f873deba2305 267 // store the data ...
palmdotax 2:f873deba2305 268 _currentMeasurement.distance = node.distance_q2/4.0f;
palmdotax 2:f873deba2305 269 _currentMeasurement.angle = (node.angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
palmdotax 2:f873deba2305 270 _currentMeasurement.quality = (node.sync_quality>>RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
palmdotax 2:f873deba2305 271 _currentMeasurement.startBit = (node.sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT);
palmdotax 4:de5a65c17664 272 ang = _currentMeasurement.angle;
palmdotax 4:de5a65c17664 273 dis = _currentMeasurement.distance;
palmdotax 4:de5a65c17664 274
palmdotax 4:de5a65c17664 275
palmdotax 4:de5a65c17664 276 if(ang>=angMin&&ang<=angMax)Data[ang] = dis;
palmdotax 4:de5a65c17664 277
palmdotax 2:f873deba2305 278 return RESULT_OK;
palmdotax 2:f873deba2305 279 }
palmdotax 2:f873deba2305 280
palmdotax 2:f873deba2305 281
palmdotax 2:f873deba2305 282 }
palmdotax 2:f873deba2305 283
palmdotax 2:f873deba2305 284 return RESULT_OPERATION_TIMEOUT;
palmdotax 2:f873deba2305 285 }
palmdotax 2:f873deba2305 286
palmdotax 2:f873deba2305 287
palmdotax 4:de5a65c17664 288 void RPLidar::setAngle(int min,int max){
palmdotax 4:de5a65c17664 289 angMin = min;
palmdotax 4:de5a65c17664 290 angMax = max;
palmdotax 4:de5a65c17664 291 }
palmdotax 2:f873deba2305 292 u_result RPLidar::_sendCommand(_u8 cmd, const void * payload, size_t payloadsize)
palmdotax 2:f873deba2305 293 {
palmdotax 2:f873deba2305 294
palmdotax 2:f873deba2305 295 rplidar_cmd_packet_t pkt_header;
palmdotax 2:f873deba2305 296 rplidar_cmd_packet_t * header = &pkt_header;
palmdotax 2:f873deba2305 297 _u8 checksum = 0;
palmdotax 2:f873deba2305 298
palmdotax 2:f873deba2305 299 if (payloadsize && payload) {
palmdotax 2:f873deba2305 300 cmd |= RPLIDAR_CMDFLAG_HAS_PAYLOAD;
palmdotax 2:f873deba2305 301 }
palmdotax 2:f873deba2305 302
palmdotax 2:f873deba2305 303 header->syncByte = RPLIDAR_CMD_SYNC_BYTE;
palmdotax 2:f873deba2305 304 header->cmd_flag = cmd;
palmdotax 2:f873deba2305 305
palmdotax 2:f873deba2305 306 // send header first
palmdotax 2:f873deba2305 307 _bined_serialdev.putc(header->syncByte );
palmdotax 2:f873deba2305 308 _bined_serialdev.putc(header->cmd_flag );
palmdotax 2:f873deba2305 309
palmdotax 2:f873deba2305 310 // _bined_serialdev->write( (uint8_t *)header, 2);
palmdotax 2:f873deba2305 311
palmdotax 2:f873deba2305 312 if (cmd & RPLIDAR_CMDFLAG_HAS_PAYLOAD) {
palmdotax 2:f873deba2305 313 checksum ^= RPLIDAR_CMD_SYNC_BYTE;
palmdotax 2:f873deba2305 314 checksum ^= cmd;
palmdotax 2:f873deba2305 315 checksum ^= (payloadsize & 0xFF);
palmdotax 2:f873deba2305 316
palmdotax 2:f873deba2305 317 // calc checksum
palmdotax 2:f873deba2305 318 for (size_t pos = 0; pos < payloadsize; ++pos) {
palmdotax 2:f873deba2305 319 checksum ^= ((_u8 *)payload)[pos];
palmdotax 2:f873deba2305 320 }
palmdotax 2:f873deba2305 321
palmdotax 2:f873deba2305 322 // send size
palmdotax 2:f873deba2305 323 _u8 sizebyte = payloadsize;
palmdotax 2:f873deba2305 324 _bined_serialdev.putc(sizebyte);
palmdotax 2:f873deba2305 325 // _bined_serialdev->write((uint8_t *)&sizebyte, 1);
palmdotax 2:f873deba2305 326
palmdotax 2:f873deba2305 327 // send payload
palmdotax 2:f873deba2305 328 // _bined_serialdev.putc((uint8_t )payload);
palmdotax 2:f873deba2305 329 // _bined_serialdev->write((uint8_t *)&payload, sizebyte);
palmdotax 2:f873deba2305 330
palmdotax 2:f873deba2305 331 // send checksum
palmdotax 2:f873deba2305 332 _bined_serialdev.putc(checksum);
palmdotax 2:f873deba2305 333 // _bined_serialdev->write((uint8_t *)&checksum, 1);
palmdotax 2:f873deba2305 334
palmdotax 2:f873deba2305 335 }
palmdotax 2:f873deba2305 336
palmdotax 2:f873deba2305 337 return RESULT_OK;
palmdotax 2:f873deba2305 338 }
palmdotax 2:f873deba2305 339
palmdotax 2:f873deba2305 340 u_result RPLidar::_waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout)
palmdotax 2:f873deba2305 341 {
palmdotax 2:f873deba2305 342 _u8 recvPos = 0;
palmdotax 2:f873deba2305 343 _u32 currentTs = timers.read_ms();
palmdotax 2:f873deba2305 344 _u32 remainingtime;
palmdotax 2:f873deba2305 345 _u8 *headerbuf = (_u8*)header;
palmdotax 2:f873deba2305 346 while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
palmdotax 2:f873deba2305 347
palmdotax 2:f873deba2305 348 int currentbyte = _bined_serialdev.getc();
palmdotax 2:f873deba2305 349 if (currentbyte<0) continue;
palmdotax 2:f873deba2305 350 switch (recvPos) {
palmdotax 2:f873deba2305 351 case 0:
palmdotax 2:f873deba2305 352 if (currentbyte != RPLIDAR_ANS_SYNC_BYTE1) {
palmdotax 2:f873deba2305 353 continue;
palmdotax 2:f873deba2305 354 }
palmdotax 2:f873deba2305 355 break;
palmdotax 2:f873deba2305 356 case 1:
palmdotax 2:f873deba2305 357 if (currentbyte != RPLIDAR_ANS_SYNC_BYTE2) {
palmdotax 2:f873deba2305 358 recvPos = 0;
palmdotax 2:f873deba2305 359 continue;
palmdotax 2:f873deba2305 360 }
palmdotax 2:f873deba2305 361 break;
palmdotax 2:f873deba2305 362 }
palmdotax 2:f873deba2305 363 headerbuf[recvPos++] = currentbyte;
palmdotax 2:f873deba2305 364
palmdotax 2:f873deba2305 365 if (recvPos == sizeof(rplidar_ans_header_t)) {
palmdotax 2:f873deba2305 366 return RESULT_OK;
palmdotax 2:f873deba2305 367 }
palmdotax 2:f873deba2305 368
palmdotax 2:f873deba2305 369
palmdotax 2:f873deba2305 370 }
palmdotax 2:f873deba2305 371
palmdotax 2:f873deba2305 372 return RESULT_OPERATION_TIMEOUT;
palmdotax 2:f873deba2305 373 }