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