João Sousa / Mbed OS romi-vff

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?

UserRevisionLine numberNew 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 }