Secret_Betago / Mbed 2 deprecated CleaningM-Palm_V2_copy

สัสชิน

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

Fork of clean_V2 by Betago

rplidar/RPlidar.cpp@8:fc70c78a443b, 2016-06-08 (annotated)

Committer:
icyzkungz
Date:
Wed Jun 08 17:19:21 2016 +0000
Revision:
8:fc70c78a443b
Parent:
5:fe76f3dae81e 
??????

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()
icyzkungz 8:fc70c78a443b 71 {
icyzkungz 8:fc70c78a443b 72 /*
icyzkungz 8:fc70c78a443b 73 if (isOpen()) {
icyzkungz 8:fc70c78a443b 74 _bined_serialdev->end();
icyzkungz 8:fc70c78a443b 75 _bined_serialdev = NULL;
icyzkungz 8:fc70c78a443b 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
icyzkungz 8:fc70c78a443b 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 5:fe76f3dae81e 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
icyzkungz 8:fc70c78a443b 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 5:fe76f3dae81e 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 u_result ans;
palmdotax 2:f873deba2305 190
palmdotax 2:f873deba2305 191 // if (!isOpen()) return RESULT_OPERATION_FAIL;
palmdotax 2:f873deba2305 192
palmdotax 2:f873deba2305 193 stop(); //force the previous operation to stop
palmdotax 2:f873deba2305 194
palmdotax 2:f873deba2305 195
icyzkungz 8:fc70c78a443b 196 ans = _sendCommand(force?RPLIDAR_CMD_FORCE_SCAN:RPLIDAR_CMD_SCAN, NULL, 0);
icyzkungz 8:fc70c78a443b 197 if (IS_FAIL(ans)) return ans;
palmdotax 2:f873deba2305 198
icyzkungz 8:fc70c78a443b 199 // waiting for confirmation
icyzkungz 8:fc70c78a443b 200 rplidar_ans_header_t response_header;
icyzkungz 8:fc70c78a443b 201 if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
icyzkungz 8:fc70c78a443b 202 return ans;
icyzkungz 8:fc70c78a443b 203 }
palmdotax 2:f873deba2305 204
icyzkungz 8:fc70c78a443b 205 // verify whether we got a correct header
icyzkungz 8:fc70c78a443b 206 if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT) {
icyzkungz 8:fc70c78a443b 207 return RESULT_INVALID_DATA;
icyzkungz 8:fc70c78a443b 208 }
palmdotax 2:f873deba2305 209
icyzkungz 8:fc70c78a443b 210 if (response_header.size < sizeof(rplidar_response_measurement_node_t)) {
icyzkungz 8:fc70c78a443b 211 return RESULT_INVALID_DATA;
icyzkungz 8:fc70c78a443b 212 }
palmdotax 2:f873deba2305 213
palmdotax 2:f873deba2305 214 return RESULT_OK;
palmdotax 2:f873deba2305 215 }
palmdotax 2:f873deba2305 216
palmdotax 2:f873deba2305 217 // wait for one sample point to arrive
palmdotax 2:f873deba2305 218 u_result RPLidar::waitPoint(_u32 timeout)
palmdotax 2:f873deba2305 219 {
icyzkungz 8:fc70c78a443b 220 _u32 currentTs = timers.read_ms();
icyzkungz 8:fc70c78a443b 221 _u32 remainingtime;
icyzkungz 8:fc70c78a443b 222 rplidar_response_measurement_node_t node;
icyzkungz 8:fc70c78a443b 223 _u8 *nodebuf = (_u8*)&node;
palmdotax 2:f873deba2305 224
icyzkungz 8:fc70c78a443b 225 _u8 recvPos = 0;
palmdotax 2:f873deba2305 226
icyzkungz 8:fc70c78a443b 227 while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
palmdotax 5:fe76f3dae81e 228 int currentbyte = _bined_serialdev->getc();
palmdotax 2:f873deba2305 229 if (currentbyte<0) continue;
palmdotax 2:f873deba2305 230 //Serial.println(currentbyte);
palmdotax 2:f873deba2305 231 switch (recvPos) {
icyzkungz 8:fc70c78a443b 232 case 0: { // expect the sync bit and its reverse in this byte {
icyzkungz 8:fc70c78a443b 233 _u8 tmp = (currentbyte>>1);
icyzkungz 8:fc70c78a443b 234 if ( (tmp ^ currentbyte) & 0x1 ) {
icyzkungz 8:fc70c78a443b 235 // pass
icyzkungz 8:fc70c78a443b 236 } else {
icyzkungz 8:fc70c78a443b 237 continue;
icyzkungz 8:fc70c78a443b 238 }
palmdotax 2:f873deba2305 239
icyzkungz 8:fc70c78a443b 240 }
icyzkungz 8:fc70c78a443b 241 break;
icyzkungz 8:fc70c78a443b 242 case 1: { // expect the highest bit to be 1
icyzkungz 8:fc70c78a443b 243 if (currentbyte & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) {
icyzkungz 8:fc70c78a443b 244 // pass
icyzkungz 8:fc70c78a443b 245 } else {
icyzkungz 8:fc70c78a443b 246 recvPos = 0;
icyzkungz 8:fc70c78a443b 247 continue;
palmdotax 2:f873deba2305 248 }
icyzkungz 8:fc70c78a443b 249 }
icyzkungz 8:fc70c78a443b 250 break;
icyzkungz 8:fc70c78a443b 251 }
icyzkungz 8:fc70c78a443b 252 nodebuf[recvPos++] = currentbyte;
icyzkungz 8:fc70c78a443b 253 if (recvPos == sizeof(rplidar_response_measurement_node_t)) {
icyzkungz 8:fc70c78a443b 254 // store the data ...
icyzkungz 8:fc70c78a443b 255 _currentMeasurement.distance = node.distance_q2/4.0f;
icyzkungz 8:fc70c78a443b 256 _currentMeasurement.angle = (node.angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
icyzkungz 8:fc70c78a443b 257 _currentMeasurement.quality = (node.sync_quality>>RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
icyzkungz 8:fc70c78a443b 258 _currentMeasurement.startBit = (node.sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT);
icyzkungz 8:fc70c78a443b 259 ang = _currentMeasurement.angle;
icyzkungz 8:fc70c78a443b 260 dis = _currentMeasurement.distance;
icyzkungz 8:fc70c78a443b 261
icyzkungz 8:fc70c78a443b 262 //Data[ang] = dis;
icyzkungz 8:fc70c78a443b 263 if(ang>=angMin&&ang<=angMax) Data[ang] = dis;
icyzkungz 8:fc70c78a443b 264
icyzkungz 8:fc70c78a443b 265 return RESULT_OK;
icyzkungz 8:fc70c78a443b 266 }
palmdotax 4:de5a65c17664 267
palmdotax 4:de5a65c17664 268
icyzkungz 8:fc70c78a443b 269 }
palmdotax 2:f873deba2305 270
icyzkungz 8:fc70c78a443b 271 return RESULT_OPERATION_TIMEOUT;
palmdotax 2:f873deba2305 272 }
palmdotax 2:f873deba2305 273
palmdotax 2:f873deba2305 274
icyzkungz 8:fc70c78a443b 275 void RPLidar::setAngle(int min,int max)
icyzkungz 8:fc70c78a443b 276 {
icyzkungz 8:fc70c78a443b 277 angMin = min;
icyzkungz 8:fc70c78a443b 278 angMax = max;
palmdotax 4:de5a65c17664 279 }
icyzkungz 8:fc70c78a443b 280 void RPLidar::get_lidar(int* data)
icyzkungz 8:fc70c78a443b 281 {
icyzkungz 8:fc70c78a443b 282 if (!IS_OK(waitPoint())) startScan();
icyzkungz 8:fc70c78a443b 283 // data = Data;
palmdotax 5:fe76f3dae81e 284 }
palmdotax 2:f873deba2305 285 u_result RPLidar::_sendCommand(_u8 cmd, const void * payload, size_t payloadsize)
palmdotax 2:f873deba2305 286 {
palmdotax 2:f873deba2305 287
palmdotax 2:f873deba2305 288 rplidar_cmd_packet_t pkt_header;
palmdotax 2:f873deba2305 289 rplidar_cmd_packet_t * header = &pkt_header;
palmdotax 2:f873deba2305 290 _u8 checksum = 0;
palmdotax 2:f873deba2305 291
palmdotax 2:f873deba2305 292 if (payloadsize && payload) {
palmdotax 2:f873deba2305 293 cmd |= RPLIDAR_CMDFLAG_HAS_PAYLOAD;
palmdotax 2:f873deba2305 294 }
palmdotax 2:f873deba2305 295
palmdotax 2:f873deba2305 296 header->syncByte = RPLIDAR_CMD_SYNC_BYTE;
palmdotax 2:f873deba2305 297 header->cmd_flag = cmd;
palmdotax 2:f873deba2305 298
palmdotax 2:f873deba2305 299 // send header first
palmdotax 5:fe76f3dae81e 300 _bined_serialdev->putc(header->syncByte );
palmdotax 5:fe76f3dae81e 301 _bined_serialdev->putc(header->cmd_flag );
palmdotax 2:f873deba2305 302
icyzkungz 8:fc70c78a443b 303 // _bined_serialdev->write( (uint8_t *)header, 2);
palmdotax 2:f873deba2305 304
palmdotax 2:f873deba2305 305 if (cmd & RPLIDAR_CMDFLAG_HAS_PAYLOAD) {
palmdotax 2:f873deba2305 306 checksum ^= RPLIDAR_CMD_SYNC_BYTE;
palmdotax 2:f873deba2305 307 checksum ^= cmd;
palmdotax 2:f873deba2305 308 checksum ^= (payloadsize & 0xFF);
palmdotax 2:f873deba2305 309
palmdotax 2:f873deba2305 310 // calc checksum
palmdotax 2:f873deba2305 311 for (size_t pos = 0; pos < payloadsize; ++pos) {
palmdotax 2:f873deba2305 312 checksum ^= ((_u8 *)payload)[pos];
palmdotax 2:f873deba2305 313 }
palmdotax 2:f873deba2305 314
palmdotax 2:f873deba2305 315 // send size
palmdotax 2:f873deba2305 316 _u8 sizebyte = payloadsize;
palmdotax 5:fe76f3dae81e 317 _bined_serialdev->putc(sizebyte);
icyzkungz 8:fc70c78a443b 318 // _bined_serialdev->write((uint8_t *)&sizebyte, 1);
palmdotax 2:f873deba2305 319
palmdotax 2:f873deba2305 320 // send payload
icyzkungz 8:fc70c78a443b 321 // _bined_serialdev.putc((uint8_t )payload);
icyzkungz 8:fc70c78a443b 322 // _bined_serialdev->write((uint8_t *)&payload, sizebyte);
palmdotax 2:f873deba2305 323
palmdotax 2:f873deba2305 324 // send checksum
palmdotax 5:fe76f3dae81e 325 _bined_serialdev->putc(checksum);
icyzkungz 8:fc70c78a443b 326 // _bined_serialdev->write((uint8_t *)&checksum, 1);
palmdotax 2:f873deba2305 327
palmdotax 2:f873deba2305 328 }
palmdotax 2:f873deba2305 329
palmdotax 2:f873deba2305 330 return RESULT_OK;
palmdotax 2:f873deba2305 331 }
palmdotax 2:f873deba2305 332
palmdotax 2:f873deba2305 333 u_result RPLidar::_waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout)
palmdotax 2:f873deba2305 334 {
palmdotax 2:f873deba2305 335 _u8 recvPos = 0;
palmdotax 2:f873deba2305 336 _u32 currentTs = timers.read_ms();
palmdotax 2:f873deba2305 337 _u32 remainingtime;
palmdotax 2:f873deba2305 338 _u8 *headerbuf = (_u8*)header;
palmdotax 2:f873deba2305 339 while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
palmdotax 2:f873deba2305 340
palmdotax 5:fe76f3dae81e 341 int currentbyte = _bined_serialdev->getc();
palmdotax 2:f873deba2305 342 if (currentbyte<0) continue;
palmdotax 2:f873deba2305 343 switch (recvPos) {
icyzkungz 8:fc70c78a443b 344 case 0:
icyzkungz 8:fc70c78a443b 345 if (currentbyte != RPLIDAR_ANS_SYNC_BYTE1) {
icyzkungz 8:fc70c78a443b 346 continue;
icyzkungz 8:fc70c78a443b 347 }
icyzkungz 8:fc70c78a443b 348 break;
icyzkungz 8:fc70c78a443b 349 case 1:
icyzkungz 8:fc70c78a443b 350 if (currentbyte != RPLIDAR_ANS_SYNC_BYTE2) {
icyzkungz 8:fc70c78a443b 351 recvPos = 0;
icyzkungz 8:fc70c78a443b 352 continue;
icyzkungz 8:fc70c78a443b 353 }
icyzkungz 8:fc70c78a443b 354 break;
palmdotax 2:f873deba2305 355 }
palmdotax 2:f873deba2305 356 headerbuf[recvPos++] = currentbyte;
palmdotax 2:f873deba2305 357
palmdotax 2:f873deba2305 358 if (recvPos == sizeof(rplidar_ans_header_t)) {
palmdotax 2:f873deba2305 359 return RESULT_OK;
palmdotax 2:f873deba2305 360 }
palmdotax 2:f873deba2305 361
palmdotax 2:f873deba2305 362
palmdotax 2:f873deba2305 363 }
palmdotax 2:f873deba2305 364
palmdotax 2:f873deba2305 365 return RESULT_OPERATION_TIMEOUT;
palmdotax 2:f873deba2305 366 }