Tiago Correia / Mbed OS SRA2020-2021

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   BufferedSerial

rplidar/RPlidar.cpp@1:e165afe8b120, 2021-05-05 (annotated)

Committer:
yaaqobhpt
Date:
Wed May 05 13:55:51 2021 +0000
Revision:
1:e165afe8b120
Parent:
0:c25c4b67b6a1
Child:
2:faef6636d456
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yaaqobhpt 0:c25c4b67b6a1 1 /*
yaaqobhpt 0:c25c4b67b6a1 2 * RoboPeak RPLIDAR Driver for Arduino
yaaqobhpt 0:c25c4b67b6a1 3 * RoboPeak.com
yaaqobhpt 0:c25c4b67b6a1 4 *
yaaqobhpt 0:c25c4b67b6a1 5 * Copyright (c) 2014, RoboPeak
yaaqobhpt 0:c25c4b67b6a1 6 * All rights reserved.
yaaqobhpt 0:c25c4b67b6a1 7 *
yaaqobhpt 0:c25c4b67b6a1 8 * Redistribution and use in source and binary forms, with or without modification,
yaaqobhpt 0:c25c4b67b6a1 9 * are permitted provided that the following conditions are met:
yaaqobhpt 0:c25c4b67b6a1 10 *
yaaqobhpt 0:c25c4b67b6a1 11 * 1. Redistributions of source code must retain the above copyright notice,
yaaqobhpt 0:c25c4b67b6a1 12 * this list of conditions and the following disclaimer.
yaaqobhpt 0:c25c4b67b6a1 13 *
yaaqobhpt 0:c25c4b67b6a1 14 * 2. Redistributions in binary form must reproduce the above copyright notice,
yaaqobhpt 0:c25c4b67b6a1 15 * this list of conditions and the following disclaimer in the documentation
yaaqobhpt 0:c25c4b67b6a1 16 * and/or other materials provided with the distribution.
yaaqobhpt 0:c25c4b67b6a1 17 *
yaaqobhpt 0:c25c4b67b6a1 18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
yaaqobhpt 0:c25c4b67b6a1 19 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
yaaqobhpt 0:c25c4b67b6a1 20 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
yaaqobhpt 0:c25c4b67b6a1 21 * SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
yaaqobhpt 0:c25c4b67b6a1 22 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
yaaqobhpt 0:c25c4b67b6a1 23 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
yaaqobhpt 0:c25c4b67b6a1 24 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
yaaqobhpt 0:c25c4b67b6a1 25 * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
yaaqobhpt 0:c25c4b67b6a1 26 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
yaaqobhpt 0:c25c4b67b6a1 27 *
yaaqobhpt 0:c25c4b67b6a1 28 */
yaaqobhpt 0:c25c4b67b6a1 29
yaaqobhpt 0:c25c4b67b6a1 30 #include "rplidar.h"
yaaqobhpt 0:c25c4b67b6a1 31 //BufferedSerial _bined_serialdev( PA_11, PA_12); // tx, rx
yaaqobhpt 0:c25c4b67b6a1 32 Timer timers;
yaaqobhpt 0:c25c4b67b6a1 33
yaaqobhpt 0:c25c4b67b6a1 34 /*
yaaqobhpt 0:c25c4b67b6a1 35 Thread that handles reading the sensor measurements
yaaqobhpt 0:c25c4b67b6a1 36
yaaqobhpt 0:c25c4b67b6a1 37 This thread parses the buffered serial bytes into Lidar measurements.
yaaqobhpt 0:c25c4b67b6a1 38 If there's space in the mail box it adds a measurement to it. In case there
yaaqobhpt 0:c25c4b67b6a1 39 isn't space, it takes 1 element out of the mail box and then puts the new
yaaqobhpt 0:c25c4b67b6a1 40 measurement. This makes sure recent data is in the mailbox instead of really
yaaqobhpt 0:c25c4b67b6a1 41 old data. Messages are lost anyway but this way the application will read
yaaqobhpt 0:c25c4b67b6a1 42 measurements of the current robot position.
yaaqobhpt 0:c25c4b67b6a1 43 if the mail box still has 100 max messages then this means that the oldest
yaaqobhpt 0:c25c4b67b6a1 44 measurement was 50ms in a full mail box.
yaaqobhpt 0:c25c4b67b6a1 45 Note that since the thread runs every 20ms then there will be measurements
yaaqobhpt 0:c25c4b67b6a1 46 as old as 20ms.
yaaqobhpt 0:c25c4b67b6a1 47 */
yaaqobhpt 0:c25c4b67b6a1 48 void RPLidar::Thread_readSensor(void)
yaaqobhpt 0:c25c4b67b6a1 49 {
yaaqobhpt 0:c25c4b67b6a1 50
yaaqobhpt 0:c25c4b67b6a1 51 //pc->printf("starting thread\n");
yaaqobhpt 0:c25c4b67b6a1 52 while(1)
yaaqobhpt 0:c25c4b67b6a1 53 {
yaaqobhpt 0:c25c4b67b6a1 54
yaaqobhpt 0:c25c4b67b6a1 55 mutex_measurements.lock();
yaaqobhpt 0:c25c4b67b6a1 56 while (IS_OK(pollPoint()))
yaaqobhpt 0:c25c4b67b6a1 57 {
yaaqobhpt 0:c25c4b67b6a1 58
yaaqobhpt 0:c25c4b67b6a1 59 currentMeasurement_fromThread = getCurrentPoint();
yaaqobhpt 0:c25c4b67b6a1 60 newMeasurement = 1;
yaaqobhpt 0:c25c4b67b6a1 61
yaaqobhpt 0:c25c4b67b6a1 62
yaaqobhpt 1:e165afe8b120 63 if(!mail_box.full())
yaaqobhpt 0:c25c4b67b6a1 64 {
yaaqobhpt 0:c25c4b67b6a1 65 struct RPLidarMeasurement current = getCurrentPoint();
yaaqobhpt 0:c25c4b67b6a1 66 struct RPLidarMeasurement *mail = mail_box.alloc();
yaaqobhpt 0:c25c4b67b6a1 67 if(mail == NULL)
yaaqobhpt 0:c25c4b67b6a1 68 {
yaaqobhpt 0:c25c4b67b6a1 69 //there was an error allocating space in heap
yaaqobhpt 0:c25c4b67b6a1 70 continue;
yaaqobhpt 0:c25c4b67b6a1 71 }
yaaqobhpt 0:c25c4b67b6a1 72 mail->distance = current.distance;
yaaqobhpt 0:c25c4b67b6a1 73 mail->angle = current.angle;
yaaqobhpt 0:c25c4b67b6a1 74 mail->quality = current.quality;
yaaqobhpt 0:c25c4b67b6a1 75 mail->startBit = current.startBit;
yaaqobhpt 0:c25c4b67b6a1 76
yaaqobhpt 0:c25c4b67b6a1 77 mail_box.put(mail);
yaaqobhpt 0:c25c4b67b6a1 78
yaaqobhpt 0:c25c4b67b6a1 79 }
yaaqobhpt 0:c25c4b67b6a1 80 else
yaaqobhpt 0:c25c4b67b6a1 81 {
yaaqobhpt 0:c25c4b67b6a1 82 osEvent evt = mail_box.get();
yaaqobhpt 0:c25c4b67b6a1 83 if (evt.status == osEventMail) {
yaaqobhpt 0:c25c4b67b6a1 84 struct RPLidarMeasurement *mail = (struct RPLidarMeasurement*)evt.value.p;
yaaqobhpt 0:c25c4b67b6a1 85 mail_box.free(mail);
yaaqobhpt 0:c25c4b67b6a1 86
yaaqobhpt 0:c25c4b67b6a1 87 struct RPLidarMeasurement current = getCurrentPoint();
yaaqobhpt 0:c25c4b67b6a1 88 mail = mail_box.alloc();
yaaqobhpt 0:c25c4b67b6a1 89 if(mail == NULL)
yaaqobhpt 0:c25c4b67b6a1 90 {
yaaqobhpt 0:c25c4b67b6a1 91 //there was an error allocating space in heap
yaaqobhpt 0:c25c4b67b6a1 92 continue;
yaaqobhpt 0:c25c4b67b6a1 93 }
yaaqobhpt 0:c25c4b67b6a1 94 mail->distance = current.distance;
yaaqobhpt 0:c25c4b67b6a1 95 mail->angle = current.angle;
yaaqobhpt 0:c25c4b67b6a1 96 mail->quality = current.quality;
yaaqobhpt 0:c25c4b67b6a1 97 mail->startBit = current.startBit;
yaaqobhpt 0:c25c4b67b6a1 98
yaaqobhpt 0:c25c4b67b6a1 99 mail_box.put(mail);
yaaqobhpt 0:c25c4b67b6a1 100 }
yaaqobhpt 0:c25c4b67b6a1 101
yaaqobhpt 0:c25c4b67b6a1 102
yaaqobhpt 1:e165afe8b120 103 }
yaaqobhpt 0:c25c4b67b6a1 104 }
yaaqobhpt 0:c25c4b67b6a1 105 mutex_measurements.unlock();
yaaqobhpt 0:c25c4b67b6a1 106 wait_us(2000);
yaaqobhpt 0:c25c4b67b6a1 107 }
yaaqobhpt 0:c25c4b67b6a1 108
yaaqobhpt 0:c25c4b67b6a1 109
yaaqobhpt 0:c25c4b67b6a1 110 }
yaaqobhpt 0:c25c4b67b6a1 111
yaaqobhpt 0:c25c4b67b6a1 112 /*
yaaqobhpt 0:c25c4b67b6a1 113 Poll for new measurements in the mail box
yaaqobhpt 0:c25c4b67b6a1 114 Returns: 0 if found data, -1 if not data available.
yaaqobhpt 0:c25c4b67b6a1 115 */
yaaqobhpt 0:c25c4b67b6a1 116 int32_t RPLidar::pollSensorData(struct RPLidarMeasurement *_data)
yaaqobhpt 0:c25c4b67b6a1 117 {
yaaqobhpt 0:c25c4b67b6a1 118
yaaqobhpt 0:c25c4b67b6a1 119 mutex_measurements.lock();
yaaqobhpt 0:c25c4b67b6a1 120 if(newMeasurement)
yaaqobhpt 0:c25c4b67b6a1 121 {
yaaqobhpt 0:c25c4b67b6a1 122 //currentMeasurement_fromThread = getCurrentPoint();
yaaqobhpt 0:c25c4b67b6a1 123 _data->distance = currentMeasurement_fromThread.distance;
yaaqobhpt 0:c25c4b67b6a1 124 _data->angle = currentMeasurement_fromThread.angle;
yaaqobhpt 0:c25c4b67b6a1 125 _data->quality = currentMeasurement_fromThread.quality;
yaaqobhpt 0:c25c4b67b6a1 126 _data->startBit = currentMeasurement_fromThread.startBit;
yaaqobhpt 0:c25c4b67b6a1 127 newMeasurement = 0;
yaaqobhpt 0:c25c4b67b6a1 128 mutex_measurements.unlock();
yaaqobhpt 0:c25c4b67b6a1 129 return 0;
yaaqobhpt 0:c25c4b67b6a1 130 }
yaaqobhpt 0:c25c4b67b6a1 131 mutex_measurements.unlock();
yaaqobhpt 0:c25c4b67b6a1 132
yaaqobhpt 0:c25c4b67b6a1 133 /*osEvent evt = mail_box.get();
yaaqobhpt 0:c25c4b67b6a1 134 if (evt.status == osEventMail) {
yaaqobhpt 0:c25c4b67b6a1 135
yaaqobhpt 0:c25c4b67b6a1 136 struct RPLidarMeasurement *mail = (struct RPLidarMeasurement*)evt.value.p;
yaaqobhpt 0:c25c4b67b6a1 137 //struct RPLidarMeasurement data;
yaaqobhpt 0:c25c4b67b6a1 138 _data->distance = mail->distance;
yaaqobhpt 0:c25c4b67b6a1 139 _data->angle = mail->angle;
yaaqobhpt 0:c25c4b67b6a1 140 _data->quality = mail->quality;
yaaqobhpt 0:c25c4b67b6a1 141 _data->startBit = mail->startBit;
yaaqobhpt 0:c25c4b67b6a1 142 mail_box.free(mail);
yaaqobhpt 0:c25c4b67b6a1 143 return 0;
yaaqobhpt 0:c25c4b67b6a1 144
yaaqobhpt 0:c25c4b67b6a1 145
yaaqobhpt 0:c25c4b67b6a1 146
yaaqobhpt 0:c25c4b67b6a1 147 }*/
yaaqobhpt 0:c25c4b67b6a1 148
yaaqobhpt 0:c25c4b67b6a1 149 return -1;
yaaqobhpt 0:c25c4b67b6a1 150 }
yaaqobhpt 0:c25c4b67b6a1 151
yaaqobhpt 0:c25c4b67b6a1 152
yaaqobhpt 0:c25c4b67b6a1 153 RPLidar::RPLidar()
yaaqobhpt 0:c25c4b67b6a1 154 {
yaaqobhpt 0:c25c4b67b6a1 155
yaaqobhpt 0:c25c4b67b6a1 156
yaaqobhpt 0:c25c4b67b6a1 157 _currentMeasurement.distance = 0;
yaaqobhpt 0:c25c4b67b6a1 158 _currentMeasurement.angle = 0;
yaaqobhpt 0:c25c4b67b6a1 159 _currentMeasurement.quality = 0;
yaaqobhpt 0:c25c4b67b6a1 160 _currentMeasurement.startBit = 0;
yaaqobhpt 0:c25c4b67b6a1 161
yaaqobhpt 0:c25c4b67b6a1 162 useThread = 0;
yaaqobhpt 0:c25c4b67b6a1 163 thread.set_priority(osPriorityAboveNormal);
yaaqobhpt 0:c25c4b67b6a1 164 //Thread thread(osPriorityAboveNormal, OS_STACK_SIZE, NULL, "RPLidar");
yaaqobhpt 0:c25c4b67b6a1 165 //thread_p = &thread;
yaaqobhpt 0:c25c4b67b6a1 166 }
yaaqobhpt 0:c25c4b67b6a1 167
yaaqobhpt 0:c25c4b67b6a1 168
yaaqobhpt 0:c25c4b67b6a1 169 RPLidar::~RPLidar()
yaaqobhpt 0:c25c4b67b6a1 170 {
yaaqobhpt 0:c25c4b67b6a1 171 end();
yaaqobhpt 0:c25c4b67b6a1 172 }
yaaqobhpt 0:c25c4b67b6a1 173
yaaqobhpt 0:c25c4b67b6a1 174 // open the given serial interface and try to connect to the RPLIDAR
yaaqobhpt 0:c25c4b67b6a1 175 /*
yaaqobhpt 0:c25c4b67b6a1 176 bool RPLidar::begin(BufferedSerial &serialobj)
yaaqobhpt 0:c25c4b67b6a1 177 {
yaaqobhpt 0:c25c4b67b6a1 178
yaaqobhpt 0:c25c4b67b6a1 179 //Serial.begin(115200);
yaaqobhpt 0:c25c4b67b6a1 180
yaaqobhpt 0:c25c4b67b6a1 181 if (isOpen()) {
yaaqobhpt 0:c25c4b67b6a1 182 end();
yaaqobhpt 0:c25c4b67b6a1 183 }
yaaqobhpt 0:c25c4b67b6a1 184 _bined_serialdev = &serialobj;
yaaqobhpt 0:c25c4b67b6a1 185 // _bined_serialdev->end();
yaaqobhpt 0:c25c4b67b6a1 186 _bined_serialdev->baud(RPLIDAR_SERIAL_BAUDRATE);
yaaqobhpt 0:c25c4b67b6a1 187 }
yaaqobhpt 0:c25c4b67b6a1 188 */
yaaqobhpt 0:c25c4b67b6a1 189
yaaqobhpt 0:c25c4b67b6a1 190 void RPLidar::begin(BufferedSerial &serialobj)
yaaqobhpt 0:c25c4b67b6a1 191 {
yaaqobhpt 0:c25c4b67b6a1 192 _bined_serialdev = &serialobj;
yaaqobhpt 0:c25c4b67b6a1 193 timers.start();
yaaqobhpt 0:c25c4b67b6a1 194 _bined_serialdev->baud(RPLIDAR_SERIAL_BAUDRATE);
yaaqobhpt 0:c25c4b67b6a1 195 recvPos_g = 0;
yaaqobhpt 0:c25c4b67b6a1 196 }
yaaqobhpt 0:c25c4b67b6a1 197 // close the currently opened serial interface
yaaqobhpt 0:c25c4b67b6a1 198 void RPLidar::end()
yaaqobhpt 0:c25c4b67b6a1 199 {/*
yaaqobhpt 0:c25c4b67b6a1 200 if (isOpen()) {
yaaqobhpt 0:c25c4b67b6a1 201 _bined_serialdev->end();
yaaqobhpt 0:c25c4b67b6a1 202 _bined_serialdev = NULL;
yaaqobhpt 0:c25c4b67b6a1 203 }*/
yaaqobhpt 0:c25c4b67b6a1 204 }
yaaqobhpt 0:c25c4b67b6a1 205
yaaqobhpt 0:c25c4b67b6a1 206
yaaqobhpt 0:c25c4b67b6a1 207 // check whether the serial interface is opened
yaaqobhpt 0:c25c4b67b6a1 208 /*
yaaqobhpt 0:c25c4b67b6a1 209 bool RPLidar::isOpen()
yaaqobhpt 0:c25c4b67b6a1 210 {
yaaqobhpt 0:c25c4b67b6a1 211 return _bined_serialdev?true:false;
yaaqobhpt 0:c25c4b67b6a1 212 }
yaaqobhpt 0:c25c4b67b6a1 213 */
yaaqobhpt 0:c25c4b67b6a1 214 // ask the RPLIDAR for its health info
yaaqobhpt 0:c25c4b67b6a1 215
yaaqobhpt 0:c25c4b67b6a1 216 u_result RPLidar::getHealth(rplidar_response_device_health_t & healthinfo, _u32 timeout)
yaaqobhpt 0:c25c4b67b6a1 217 {
yaaqobhpt 0:c25c4b67b6a1 218 _u32 currentTs = timers.read_ms();
yaaqobhpt 0:c25c4b67b6a1 219 _u32 remainingtime;
yaaqobhpt 0:c25c4b67b6a1 220
yaaqobhpt 0:c25c4b67b6a1 221 _u8 *infobuf = (_u8 *)&healthinfo;
yaaqobhpt 0:c25c4b67b6a1 222 _u8 recvPos = 0;
yaaqobhpt 0:c25c4b67b6a1 223
yaaqobhpt 0:c25c4b67b6a1 224 rplidar_ans_header_t response_header;
yaaqobhpt 0:c25c4b67b6a1 225 u_result ans;
yaaqobhpt 0:c25c4b67b6a1 226
yaaqobhpt 0:c25c4b67b6a1 227
yaaqobhpt 0:c25c4b67b6a1 228 // if (!isOpen()) return RESULT_OPERATION_FAIL;
yaaqobhpt 0:c25c4b67b6a1 229
yaaqobhpt 0:c25c4b67b6a1 230 {
yaaqobhpt 0:c25c4b67b6a1 231 if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_HEALTH, NULL, 0))) {
yaaqobhpt 0:c25c4b67b6a1 232 return ans;
yaaqobhpt 0:c25c4b67b6a1 233 }
yaaqobhpt 0:c25c4b67b6a1 234
yaaqobhpt 0:c25c4b67b6a1 235 if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
yaaqobhpt 0:c25c4b67b6a1 236 return ans;
yaaqobhpt 0:c25c4b67b6a1 237 }
yaaqobhpt 0:c25c4b67b6a1 238
yaaqobhpt 0:c25c4b67b6a1 239 // verify whether we got a correct header
yaaqobhpt 0:c25c4b67b6a1 240 if (response_header.type != RPLIDAR_ANS_TYPE_DEVHEALTH) {
yaaqobhpt 0:c25c4b67b6a1 241 return RESULT_INVALID_DATA;
yaaqobhpt 0:c25c4b67b6a1 242 }
yaaqobhpt 0:c25c4b67b6a1 243
yaaqobhpt 0:c25c4b67b6a1 244 if ((response_header.size) < sizeof(rplidar_response_device_health_t)) {
yaaqobhpt 0:c25c4b67b6a1 245 return RESULT_INVALID_DATA;
yaaqobhpt 0:c25c4b67b6a1 246 }
yaaqobhpt 0:c25c4b67b6a1 247
yaaqobhpt 0:c25c4b67b6a1 248 while ((remainingtime=timers.read_ms() - currentTs) <= timeout) {
yaaqobhpt 0:c25c4b67b6a1 249 int currentbyte = _bined_serialdev->getc();
yaaqobhpt 0:c25c4b67b6a1 250 if (currentbyte < 0) continue;
yaaqobhpt 0:c25c4b67b6a1 251
yaaqobhpt 0:c25c4b67b6a1 252 infobuf[recvPos++] = currentbyte;
yaaqobhpt 0:c25c4b67b6a1 253
yaaqobhpt 0:c25c4b67b6a1 254 if (recvPos == sizeof(rplidar_response_device_health_t)) {
yaaqobhpt 0:c25c4b67b6a1 255 return RESULT_OK;
yaaqobhpt 0:c25c4b67b6a1 256 }
yaaqobhpt 0:c25c4b67b6a1 257 }
yaaqobhpt 0:c25c4b67b6a1 258 }
yaaqobhpt 0:c25c4b67b6a1 259 return RESULT_OPERATION_TIMEOUT;
yaaqobhpt 0:c25c4b67b6a1 260 }
yaaqobhpt 0:c25c4b67b6a1 261 // ask the RPLIDAR for its device info like the serial number
yaaqobhpt 0:c25c4b67b6a1 262 u_result RPLidar::getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout )
yaaqobhpt 0:c25c4b67b6a1 263 {
yaaqobhpt 0:c25c4b67b6a1 264 _u8 recvPos = 0;
yaaqobhpt 0:c25c4b67b6a1 265 _u32 currentTs = timers.read_ms();
yaaqobhpt 0:c25c4b67b6a1 266 _u32 remainingtime;
yaaqobhpt 0:c25c4b67b6a1 267 _u8 *infobuf = (_u8*)&info;
yaaqobhpt 0:c25c4b67b6a1 268 rplidar_ans_header_t response_header;
yaaqobhpt 0:c25c4b67b6a1 269 u_result ans;
yaaqobhpt 0:c25c4b67b6a1 270
yaaqobhpt 0:c25c4b67b6a1 271 // if (!isOpen()) return RESULT_OPERATION_FAIL;
yaaqobhpt 0:c25c4b67b6a1 272
yaaqobhpt 0:c25c4b67b6a1 273 {
yaaqobhpt 0:c25c4b67b6a1 274 if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_INFO,NULL,0))) {
yaaqobhpt 0:c25c4b67b6a1 275 return ans;
yaaqobhpt 0:c25c4b67b6a1 276 }
yaaqobhpt 0:c25c4b67b6a1 277
yaaqobhpt 0:c25c4b67b6a1 278 if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
yaaqobhpt 0:c25c4b67b6a1 279 return ans;
yaaqobhpt 0:c25c4b67b6a1 280 }
yaaqobhpt 0:c25c4b67b6a1 281
yaaqobhpt 0:c25c4b67b6a1 282 // verify whether we got a correct header
yaaqobhpt 0:c25c4b67b6a1 283 if (response_header.type != RPLIDAR_ANS_TYPE_DEVINFO) {
yaaqobhpt 0:c25c4b67b6a1 284 return RESULT_INVALID_DATA;
yaaqobhpt 0:c25c4b67b6a1 285 }
yaaqobhpt 0:c25c4b67b6a1 286
yaaqobhpt 0:c25c4b67b6a1 287 if (response_header.size < sizeof(rplidar_response_device_info_t)) {
yaaqobhpt 0:c25c4b67b6a1 288 return RESULT_INVALID_DATA;
yaaqobhpt 0:c25c4b67b6a1 289 }
yaaqobhpt 0:c25c4b67b6a1 290
yaaqobhpt 0:c25c4b67b6a1 291 while ((remainingtime=timers.read_ms() - currentTs) <= timeout) {
yaaqobhpt 0:c25c4b67b6a1 292 int currentbyte = _bined_serialdev->getc();
yaaqobhpt 0:c25c4b67b6a1 293 if (currentbyte<0) continue;
yaaqobhpt 0:c25c4b67b6a1 294 infobuf[recvPos++] = currentbyte;
yaaqobhpt 0:c25c4b67b6a1 295
yaaqobhpt 0:c25c4b67b6a1 296 if (recvPos == sizeof(rplidar_response_device_info_t)) {
yaaqobhpt 0:c25c4b67b6a1 297 return RESULT_OK;
yaaqobhpt 0:c25c4b67b6a1 298 }
yaaqobhpt 0:c25c4b67b6a1 299 }
yaaqobhpt 0:c25c4b67b6a1 300 }
yaaqobhpt 0:c25c4b67b6a1 301
yaaqobhpt 0:c25c4b67b6a1 302 return RESULT_OPERATION_TIMEOUT;
yaaqobhpt 0:c25c4b67b6a1 303 }
yaaqobhpt 0:c25c4b67b6a1 304
yaaqobhpt 0:c25c4b67b6a1 305 // stop the measurement operation
yaaqobhpt 0:c25c4b67b6a1 306 u_result RPLidar::stop()
yaaqobhpt 0:c25c4b67b6a1 307 {
yaaqobhpt 0:c25c4b67b6a1 308 // if (!isOpen()) return RESULT_OPERATION_FAIL;
yaaqobhpt 0:c25c4b67b6a1 309 u_result ans = _sendCommand(RPLIDAR_CMD_STOP,NULL,0);
yaaqobhpt 0:c25c4b67b6a1 310 return ans;
yaaqobhpt 0:c25c4b67b6a1 311 }
yaaqobhpt 0:c25c4b67b6a1 312
yaaqobhpt 0:c25c4b67b6a1 313
yaaqobhpt 0:c25c4b67b6a1 314
yaaqobhpt 0:c25c4b67b6a1 315 u_result RPLidar::startThreadScan()
yaaqobhpt 0:c25c4b67b6a1 316 {
yaaqobhpt 0:c25c4b67b6a1 317 startScan();
yaaqobhpt 0:c25c4b67b6a1 318 useThread = true;
yaaqobhpt 0:c25c4b67b6a1 319 thread.start(callback(this, &RPLidar::Thread_readSensor));
yaaqobhpt 0:c25c4b67b6a1 320
yaaqobhpt 0:c25c4b67b6a1 321 return RESULT_OK;
yaaqobhpt 0:c25c4b67b6a1 322 }
yaaqobhpt 0:c25c4b67b6a1 323 // start the measurement operation
yaaqobhpt 0:c25c4b67b6a1 324 /*
yaaqobhpt 0:c25c4b67b6a1 325 This was altered to also start the thread that parses measurements in the background
yaaqobhpt 0:c25c4b67b6a1 326 */
yaaqobhpt 0:c25c4b67b6a1 327 u_result RPLidar::startScan(bool force, _u32 timeout)
yaaqobhpt 0:c25c4b67b6a1 328 {
yaaqobhpt 0:c25c4b67b6a1 329 //pc->printf("RPLidar::startScan\n");
yaaqobhpt 0:c25c4b67b6a1 330 u_result ans;
yaaqobhpt 0:c25c4b67b6a1 331
yaaqobhpt 0:c25c4b67b6a1 332 // if (!isOpen()) return RESULT_OPERATION_FAIL;
yaaqobhpt 0:c25c4b67b6a1 333
yaaqobhpt 0:c25c4b67b6a1 334 stop(); //force the previous operation to stop
yaaqobhpt 0:c25c4b67b6a1 335
yaaqobhpt 0:c25c4b67b6a1 336
yaaqobhpt 0:c25c4b67b6a1 337 ans = _sendCommand(force?RPLIDAR_CMD_FORCE_SCAN:RPLIDAR_CMD_SCAN, NULL, 0);
yaaqobhpt 0:c25c4b67b6a1 338 if (IS_FAIL(ans)) return ans;
yaaqobhpt 0:c25c4b67b6a1 339
yaaqobhpt 0:c25c4b67b6a1 340 // waiting for confirmation
yaaqobhpt 0:c25c4b67b6a1 341 rplidar_ans_header_t response_header;
yaaqobhpt 0:c25c4b67b6a1 342 if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
yaaqobhpt 0:c25c4b67b6a1 343 return ans;
yaaqobhpt 0:c25c4b67b6a1 344 }
yaaqobhpt 0:c25c4b67b6a1 345
yaaqobhpt 0:c25c4b67b6a1 346 // verify whether we got a correct header
yaaqobhpt 0:c25c4b67b6a1 347 if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT) {
yaaqobhpt 0:c25c4b67b6a1 348 return RESULT_INVALID_DATA;
yaaqobhpt 0:c25c4b67b6a1 349 }
yaaqobhpt 0:c25c4b67b6a1 350
yaaqobhpt 0:c25c4b67b6a1 351 if (response_header.size < sizeof(rplidar_response_measurement_node_t)) {
yaaqobhpt 0:c25c4b67b6a1 352 return RESULT_INVALID_DATA;
yaaqobhpt 0:c25c4b67b6a1 353 }
yaaqobhpt 0:c25c4b67b6a1 354
yaaqobhpt 0:c25c4b67b6a1 355
yaaqobhpt 0:c25c4b67b6a1 356 return RESULT_OK;
yaaqobhpt 0:c25c4b67b6a1 357 }
yaaqobhpt 0:c25c4b67b6a1 358
yaaqobhpt 0:c25c4b67b6a1 359 // wait for one sample point to arrive
yaaqobhpt 0:c25c4b67b6a1 360 /*
yaaqobhpt 0:c25c4b67b6a1 361 Do not use if startScan was called with starThread == 1!
yaaqobhpt 0:c25c4b67b6a1 362 */
yaaqobhpt 0:c25c4b67b6a1 363 u_result RPLidar::waitPoint(_u32 timeout)
yaaqobhpt 0:c25c4b67b6a1 364 {
yaaqobhpt 0:c25c4b67b6a1 365
yaaqobhpt 0:c25c4b67b6a1 366 if(useThread)
yaaqobhpt 0:c25c4b67b6a1 367 return RESULT_OPERATION_NOT_SUPPORT;
yaaqobhpt 0:c25c4b67b6a1 368
yaaqobhpt 0:c25c4b67b6a1 369 _u32 currentTs = timers.read_ms();
yaaqobhpt 0:c25c4b67b6a1 370 _u32 remainingtime;
yaaqobhpt 0:c25c4b67b6a1 371 rplidar_response_measurement_node_t node;
yaaqobhpt 0:c25c4b67b6a1 372 _u8 *nodebuf = (_u8*)&node;
yaaqobhpt 0:c25c4b67b6a1 373
yaaqobhpt 0:c25c4b67b6a1 374 _u8 recvPos = 0;
yaaqobhpt 0:c25c4b67b6a1 375
yaaqobhpt 0:c25c4b67b6a1 376 while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
yaaqobhpt 0:c25c4b67b6a1 377 int currentbyte = _bined_serialdev->getc();
yaaqobhpt 0:c25c4b67b6a1 378 if (currentbyte<0) continue;
yaaqobhpt 0:c25c4b67b6a1 379 //Serial.println(currentbyte);
yaaqobhpt 0:c25c4b67b6a1 380 switch (recvPos) {
yaaqobhpt 0:c25c4b67b6a1 381 case 0: // expect the sync bit and its reverse in this byte {
yaaqobhpt 0:c25c4b67b6a1 382 {
yaaqobhpt 0:c25c4b67b6a1 383 _u8 tmp = (currentbyte>>1);
yaaqobhpt 0:c25c4b67b6a1 384 if ( (tmp ^ currentbyte) & 0x1 ) {
yaaqobhpt 0:c25c4b67b6a1 385 // pass
yaaqobhpt 0:c25c4b67b6a1 386 } else {
yaaqobhpt 0:c25c4b67b6a1 387 continue;
yaaqobhpt 0:c25c4b67b6a1 388 }
yaaqobhpt 0:c25c4b67b6a1 389
yaaqobhpt 0:c25c4b67b6a1 390 }
yaaqobhpt 0:c25c4b67b6a1 391 break;
yaaqobhpt 0:c25c4b67b6a1 392 case 1: // expect the highest bit to be 1
yaaqobhpt 0:c25c4b67b6a1 393 {
yaaqobhpt 0:c25c4b67b6a1 394 if (currentbyte & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) {
yaaqobhpt 0:c25c4b67b6a1 395 // pass
yaaqobhpt 0:c25c4b67b6a1 396 } else {
yaaqobhpt 0:c25c4b67b6a1 397 recvPos = 0;
yaaqobhpt 0:c25c4b67b6a1 398 continue;
yaaqobhpt 0:c25c4b67b6a1 399 }
yaaqobhpt 0:c25c4b67b6a1 400 }
yaaqobhpt 0:c25c4b67b6a1 401 break;
yaaqobhpt 0:c25c4b67b6a1 402 }
yaaqobhpt 0:c25c4b67b6a1 403 nodebuf[recvPos++] = currentbyte;
yaaqobhpt 0:c25c4b67b6a1 404 if (recvPos == sizeof(rplidar_response_measurement_node_t)) {
yaaqobhpt 0:c25c4b67b6a1 405 // store the data ...
yaaqobhpt 0:c25c4b67b6a1 406 _currentMeasurement.distance = node.distance_q2/4.0f;
yaaqobhpt 0:c25c4b67b6a1 407 _currentMeasurement.angle = (node.angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
yaaqobhpt 0:c25c4b67b6a1 408 _currentMeasurement.quality = (node.sync_quality>>RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
yaaqobhpt 0:c25c4b67b6a1 409 _currentMeasurement.startBit = (node.sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT);
yaaqobhpt 0:c25c4b67b6a1 410 ang = _currentMeasurement.angle;
yaaqobhpt 0:c25c4b67b6a1 411 dis = _currentMeasurement.distance;
yaaqobhpt 0:c25c4b67b6a1 412
yaaqobhpt 0:c25c4b67b6a1 413
yaaqobhpt 0:c25c4b67b6a1 414 if(ang>=angMin&&ang<=angMax)Data[ang] = dis;
yaaqobhpt 0:c25c4b67b6a1 415
yaaqobhpt 0:c25c4b67b6a1 416 return RESULT_OK;
yaaqobhpt 0:c25c4b67b6a1 417 }
yaaqobhpt 0:c25c4b67b6a1 418
yaaqobhpt 0:c25c4b67b6a1 419
yaaqobhpt 0:c25c4b67b6a1 420 }
yaaqobhpt 0:c25c4b67b6a1 421
yaaqobhpt 0:c25c4b67b6a1 422 return RESULT_OPERATION_TIMEOUT;
yaaqobhpt 0:c25c4b67b6a1 423 }
yaaqobhpt 0:c25c4b67b6a1 424
yaaqobhpt 0:c25c4b67b6a1 425
yaaqobhpt 0:c25c4b67b6a1 426 /*
yaaqobhpt 0:c25c4b67b6a1 427 This is very similar to waitPoint and it's only to be used by the thread,
yaaqobhpt 0:c25c4b67b6a1 428 hence why it private.
yaaqobhpt 0:c25c4b67b6a1 429
yaaqobhpt 0:c25c4b67b6a1 430 It checks for data in the buffered serial until it finds a message or there
yaaqobhpt 0:c25c4b67b6a1 431 are no more bytes.
yaaqobhpt 0:c25c4b67b6a1 432 If it finds a message it returns RESULT_OK
yaaqobhpt 0:c25c4b67b6a1 433 If there are no more bytes it returns RESULT_OPERATION_TIMEOUT.
yaaqobhpt 0:c25c4b67b6a1 434
yaaqobhpt 0:c25c4b67b6a1 435 The state of the parsing is saved so it can continue parsing a measurement
yaaqobhpt 0:c25c4b67b6a1 436 midway. (Note that does not mean it's re-entrant and should only be used
yaaqobhpt 0:c25c4b67b6a1 437 in the same context)
yaaqobhpt 0:c25c4b67b6a1 438 */
yaaqobhpt 0:c25c4b67b6a1 439 u_result RPLidar::pollPoint()
yaaqobhpt 0:c25c4b67b6a1 440 {
yaaqobhpt 0:c25c4b67b6a1 441
yaaqobhpt 0:c25c4b67b6a1 442 //_u32 currentTs = timers.read_ms();
yaaqobhpt 0:c25c4b67b6a1 443 _u32 remainingtime;
yaaqobhpt 0:c25c4b67b6a1 444 //rplidar_response_measurement_node_t node;
yaaqobhpt 0:c25c4b67b6a1 445 _u8 *nodebuf = (_u8*)&(node_g);
yaaqobhpt 0:c25c4b67b6a1 446
yaaqobhpt 0:c25c4b67b6a1 447
yaaqobhpt 0:c25c4b67b6a1 448 int currentbyte;
yaaqobhpt 0:c25c4b67b6a1 449 while(_bined_serialdev->readable() > 0)
yaaqobhpt 0:c25c4b67b6a1 450 {
yaaqobhpt 0:c25c4b67b6a1 451 //while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
yaaqobhpt 0:c25c4b67b6a1 452 currentbyte = _bined_serialdev->getc();
yaaqobhpt 0:c25c4b67b6a1 453 //if (_bined_serialdev->readable() == 0) continue;
yaaqobhpt 0:c25c4b67b6a1 454 //Serial.println(currentbyte);
yaaqobhpt 0:c25c4b67b6a1 455 switch (recvPos_g) {
yaaqobhpt 0:c25c4b67b6a1 456 case 0: // expect the sync bit and its reverse in this byte {
yaaqobhpt 0:c25c4b67b6a1 457 {
yaaqobhpt 0:c25c4b67b6a1 458 _u8 tmp = (currentbyte>>1);
yaaqobhpt 0:c25c4b67b6a1 459 if ( (tmp ^ currentbyte) & 0x1 ) {
yaaqobhpt 0:c25c4b67b6a1 460 // pass
yaaqobhpt 0:c25c4b67b6a1 461
yaaqobhpt 0:c25c4b67b6a1 462 } else {
yaaqobhpt 0:c25c4b67b6a1 463 continue;
yaaqobhpt 0:c25c4b67b6a1 464 }
yaaqobhpt 0:c25c4b67b6a1 465
yaaqobhpt 0:c25c4b67b6a1 466 }
yaaqobhpt 0:c25c4b67b6a1 467 break;
yaaqobhpt 0:c25c4b67b6a1 468 case 1: // expect the highest bit to be 1
yaaqobhpt 0:c25c4b67b6a1 469 {
yaaqobhpt 0:c25c4b67b6a1 470 if (currentbyte & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) {
yaaqobhpt 0:c25c4b67b6a1 471 // pass
yaaqobhpt 0:c25c4b67b6a1 472 } else {
yaaqobhpt 0:c25c4b67b6a1 473 recvPos_g = 0;
yaaqobhpt 0:c25c4b67b6a1 474 continue;
yaaqobhpt 0:c25c4b67b6a1 475 }
yaaqobhpt 0:c25c4b67b6a1 476 }
yaaqobhpt 0:c25c4b67b6a1 477 break;
yaaqobhpt 0:c25c4b67b6a1 478 }
yaaqobhpt 0:c25c4b67b6a1 479 nodebuf[recvPos_g++] = currentbyte;
yaaqobhpt 0:c25c4b67b6a1 480 if (recvPos_g == sizeof(rplidar_response_measurement_node_t)) {
yaaqobhpt 0:c25c4b67b6a1 481 recvPos_g = 0;
yaaqobhpt 0:c25c4b67b6a1 482 // store the data ...
yaaqobhpt 0:c25c4b67b6a1 483 _currentMeasurement.distance = node_g.distance_q2/4.0f;
yaaqobhpt 0:c25c4b67b6a1 484 _currentMeasurement.angle = (node_g.angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
yaaqobhpt 0:c25c4b67b6a1 485 _currentMeasurement.quality = (node_g.sync_quality>>RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
yaaqobhpt 0:c25c4b67b6a1 486 _currentMeasurement.startBit = (node_g.sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT);
yaaqobhpt 0:c25c4b67b6a1 487 ang = _currentMeasurement.angle;
yaaqobhpt 0:c25c4b67b6a1 488 dis = _currentMeasurement.distance;
yaaqobhpt 0:c25c4b67b6a1 489
yaaqobhpt 0:c25c4b67b6a1 490
yaaqobhpt 0:c25c4b67b6a1 491 if(ang>=angMin&&ang<=angMax)Data[ang] = dis;
yaaqobhpt 0:c25c4b67b6a1 492
yaaqobhpt 0:c25c4b67b6a1 493 return RESULT_OK;
yaaqobhpt 0:c25c4b67b6a1 494 }
yaaqobhpt 0:c25c4b67b6a1 495
yaaqobhpt 0:c25c4b67b6a1 496
yaaqobhpt 0:c25c4b67b6a1 497 }//while(_bined_serialdev->readable() > 0);
yaaqobhpt 0:c25c4b67b6a1 498
yaaqobhpt 0:c25c4b67b6a1 499 return RESULT_OPERATION_TIMEOUT;
yaaqobhpt 0:c25c4b67b6a1 500 }
yaaqobhpt 0:c25c4b67b6a1 501
yaaqobhpt 0:c25c4b67b6a1 502
yaaqobhpt 0:c25c4b67b6a1 503 void RPLidar::setAngle(int min,int max){
yaaqobhpt 0:c25c4b67b6a1 504 angMin = min;
yaaqobhpt 0:c25c4b67b6a1 505 angMax = max;
yaaqobhpt 0:c25c4b67b6a1 506 }
yaaqobhpt 0:c25c4b67b6a1 507 void RPLidar::get_lidar(){
yaaqobhpt 0:c25c4b67b6a1 508 if (!IS_OK(waitPoint())) startScan();
yaaqobhpt 0:c25c4b67b6a1 509 }
yaaqobhpt 0:c25c4b67b6a1 510 u_result RPLidar::_sendCommand(_u8 cmd, const void * payload, size_t payloadsize)
yaaqobhpt 0:c25c4b67b6a1 511 {
yaaqobhpt 0:c25c4b67b6a1 512
yaaqobhpt 0:c25c4b67b6a1 513 rplidar_cmd_packet_t pkt_header;
yaaqobhpt 0:c25c4b67b6a1 514 rplidar_cmd_packet_t * header = &pkt_header;
yaaqobhpt 0:c25c4b67b6a1 515 _u8 checksum = 0;
yaaqobhpt 0:c25c4b67b6a1 516
yaaqobhpt 0:c25c4b67b6a1 517 if (payloadsize && payload) {
yaaqobhpt 0:c25c4b67b6a1 518 cmd |= RPLIDAR_CMDFLAG_HAS_PAYLOAD;
yaaqobhpt 0:c25c4b67b6a1 519 }
yaaqobhpt 0:c25c4b67b6a1 520
yaaqobhpt 0:c25c4b67b6a1 521 header->syncByte = RPLIDAR_CMD_SYNC_BYTE;
yaaqobhpt 0:c25c4b67b6a1 522 header->cmd_flag = cmd;
yaaqobhpt 0:c25c4b67b6a1 523
yaaqobhpt 0:c25c4b67b6a1 524 // send header first
yaaqobhpt 0:c25c4b67b6a1 525 _bined_serialdev->putc(header->syncByte );
yaaqobhpt 0:c25c4b67b6a1 526 _bined_serialdev->putc(header->cmd_flag );
yaaqobhpt 0:c25c4b67b6a1 527
yaaqobhpt 0:c25c4b67b6a1 528 // _bined_serialdev->write( (uint8_t *)header, 2);
yaaqobhpt 0:c25c4b67b6a1 529
yaaqobhpt 0:c25c4b67b6a1 530 if (cmd & RPLIDAR_CMDFLAG_HAS_PAYLOAD) {
yaaqobhpt 0:c25c4b67b6a1 531 checksum ^= RPLIDAR_CMD_SYNC_BYTE;
yaaqobhpt 0:c25c4b67b6a1 532 checksum ^= cmd;
yaaqobhpt 0:c25c4b67b6a1 533 checksum ^= (payloadsize & 0xFF);
yaaqobhpt 0:c25c4b67b6a1 534
yaaqobhpt 0:c25c4b67b6a1 535 // calc checksum
yaaqobhpt 0:c25c4b67b6a1 536 for (size_t pos = 0; pos < payloadsize; ++pos) {
yaaqobhpt 0:c25c4b67b6a1 537 checksum ^= ((_u8 *)payload)[pos];
yaaqobhpt 0:c25c4b67b6a1 538 }
yaaqobhpt 0:c25c4b67b6a1 539
yaaqobhpt 0:c25c4b67b6a1 540 // send size
yaaqobhpt 0:c25c4b67b6a1 541 _u8 sizebyte = payloadsize;
yaaqobhpt 0:c25c4b67b6a1 542 _bined_serialdev->putc(sizebyte);
yaaqobhpt 0:c25c4b67b6a1 543 // _bined_serialdev->write((uint8_t *)&sizebyte, 1);
yaaqobhpt 0:c25c4b67b6a1 544
yaaqobhpt 0:c25c4b67b6a1 545 // send payload
yaaqobhpt 0:c25c4b67b6a1 546 // _bined_serialdev.putc((uint8_t )payload);
yaaqobhpt 0:c25c4b67b6a1 547 // _bined_serialdev->write((uint8_t *)&payload, sizebyte);
yaaqobhpt 0:c25c4b67b6a1 548
yaaqobhpt 0:c25c4b67b6a1 549 // send checksum
yaaqobhpt 0:c25c4b67b6a1 550 _bined_serialdev->putc(checksum);
yaaqobhpt 0:c25c4b67b6a1 551 // _bined_serialdev->write((uint8_t *)&checksum, 1);
yaaqobhpt 0:c25c4b67b6a1 552
yaaqobhpt 0:c25c4b67b6a1 553 }
yaaqobhpt 0:c25c4b67b6a1 554
yaaqobhpt 0:c25c4b67b6a1 555 return RESULT_OK;
yaaqobhpt 0:c25c4b67b6a1 556 }
yaaqobhpt 0:c25c4b67b6a1 557
yaaqobhpt 0:c25c4b67b6a1 558 u_result RPLidar::_waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout)
yaaqobhpt 0:c25c4b67b6a1 559 {
yaaqobhpt 0:c25c4b67b6a1 560 _u8 recvPos = 0;
yaaqobhpt 0:c25c4b67b6a1 561 _u32 currentTs = timers.read_ms();
yaaqobhpt 0:c25c4b67b6a1 562 _u32 remainingtime;
yaaqobhpt 0:c25c4b67b6a1 563 _u8 *headerbuf = (_u8*)header;
yaaqobhpt 0:c25c4b67b6a1 564 while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
yaaqobhpt 0:c25c4b67b6a1 565
yaaqobhpt 0:c25c4b67b6a1 566 int currentbyte = _bined_serialdev->getc();
yaaqobhpt 0:c25c4b67b6a1 567 if (currentbyte<0) continue;
yaaqobhpt 0:c25c4b67b6a1 568 switch (recvPos) {
yaaqobhpt 0:c25c4b67b6a1 569 case 0:
yaaqobhpt 0:c25c4b67b6a1 570 if (currentbyte != RPLIDAR_ANS_SYNC_BYTE1) {
yaaqobhpt 0:c25c4b67b6a1 571 continue;
yaaqobhpt 0:c25c4b67b6a1 572 }
yaaqobhpt 0:c25c4b67b6a1 573 break;
yaaqobhpt 0:c25c4b67b6a1 574 case 1:
yaaqobhpt 0:c25c4b67b6a1 575 if (currentbyte != RPLIDAR_ANS_SYNC_BYTE2) {
yaaqobhpt 0:c25c4b67b6a1 576 recvPos = 0;
yaaqobhpt 0:c25c4b67b6a1 577 continue;
yaaqobhpt 0:c25c4b67b6a1 578 }
yaaqobhpt 0:c25c4b67b6a1 579 break;
yaaqobhpt 0:c25c4b67b6a1 580 }
yaaqobhpt 0:c25c4b67b6a1 581 headerbuf[recvPos++] = currentbyte;
yaaqobhpt 0:c25c4b67b6a1 582
yaaqobhpt 0:c25c4b67b6a1 583 if (recvPos == sizeof(rplidar_ans_header_t)) {
yaaqobhpt 0:c25c4b67b6a1 584 return RESULT_OK;
yaaqobhpt 0:c25c4b67b6a1 585 }
yaaqobhpt 0:c25c4b67b6a1 586
yaaqobhpt 0:c25c4b67b6a1 587
yaaqobhpt 0:c25c4b67b6a1 588 }
yaaqobhpt 0:c25c4b67b6a1 589
yaaqobhpt 0:c25c4b67b6a1 590 return RESULT_OPERATION_TIMEOUT;
yaaqobhpt 0:c25c4b67b6a1 591 }
yaaqobhpt 0:c25c4b67b6a1 592