João Sousa / Mbed OS romi-vff

sra-romi

Dependencies:   BufferedSerial Matrix

rplidar/RPlidar.cpp@0:2b691d200d6f, 2019-04-09 (annotated)

Committer:
LuisRA
Date:
Tue Apr 09 17:53:31 2019 +0000
Revision:
0:2b691d200d6f
Child:
2:e27733eaa594
initial commit

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
LuisRA 0:2b691d200d6f 55 while (IS_OK(pollPoint()))
LuisRA 0:2b691d200d6f 56 {
LuisRA 0:2b691d200d6f 57
LuisRA 0:2b691d200d6f 58 if(!mail_box.full())
LuisRA 0:2b691d200d6f 59 {
LuisRA 0:2b691d200d6f 60 struct RPLidarMeasurement current = getCurrentPoint();
LuisRA 0:2b691d200d6f 61 struct RPLidarMeasurement *mail = mail_box.alloc();
LuisRA 0:2b691d200d6f 62 if(mail == NULL)
LuisRA 0:2b691d200d6f 63 {
LuisRA 0:2b691d200d6f 64 //there was an error allocating space in heap
LuisRA 0:2b691d200d6f 65 continue;
LuisRA 0:2b691d200d6f 66 }
LuisRA 0:2b691d200d6f 67 mail->distance = current.distance;
LuisRA 0:2b691d200d6f 68 mail->angle = current.angle;
LuisRA 0:2b691d200d6f 69 mail->quality = current.quality;
LuisRA 0:2b691d200d6f 70 mail->startBit = current.startBit;
LuisRA 0:2b691d200d6f 71
LuisRA 0:2b691d200d6f 72 mail_box.put(mail);
LuisRA 0:2b691d200d6f 73
LuisRA 0:2b691d200d6f 74 }
LuisRA 0:2b691d200d6f 75 else
LuisRA 0:2b691d200d6f 76 {
LuisRA 0:2b691d200d6f 77 osEvent evt = mail_box.get();
LuisRA 0:2b691d200d6f 78 if (evt.status == osEventMail) {
LuisRA 0:2b691d200d6f 79 struct RPLidarMeasurement *mail = (struct RPLidarMeasurement*)evt.value.p;
LuisRA 0:2b691d200d6f 80 mail_box.free(mail);
LuisRA 0:2b691d200d6f 81
LuisRA 0:2b691d200d6f 82 struct RPLidarMeasurement current = getCurrentPoint();
LuisRA 0:2b691d200d6f 83 mail = mail_box.alloc();
LuisRA 0:2b691d200d6f 84 if(mail == NULL)
LuisRA 0:2b691d200d6f 85 {
LuisRA 0:2b691d200d6f 86 //there was an error allocating space in heap
LuisRA 0:2b691d200d6f 87 continue;
LuisRA 0:2b691d200d6f 88 }
LuisRA 0:2b691d200d6f 89 mail->distance = current.distance;
LuisRA 0:2b691d200d6f 90 mail->angle = current.angle;
LuisRA 0:2b691d200d6f 91 mail->quality = current.quality;
LuisRA 0:2b691d200d6f 92 mail->startBit = current.startBit;
LuisRA 0:2b691d200d6f 93
LuisRA 0:2b691d200d6f 94 mail_box.put(mail);
LuisRA 0:2b691d200d6f 95 }
LuisRA 0:2b691d200d6f 96
LuisRA 0:2b691d200d6f 97
LuisRA 0:2b691d200d6f 98 }
LuisRA 0:2b691d200d6f 99 }
LuisRA 0:2b691d200d6f 100 wait(0.02);
LuisRA 0:2b691d200d6f 101 }
LuisRA 0:2b691d200d6f 102
LuisRA 0:2b691d200d6f 103
LuisRA 0:2b691d200d6f 104 }
LuisRA 0:2b691d200d6f 105
LuisRA 0:2b691d200d6f 106 /*
LuisRA 0:2b691d200d6f 107 Poll for new measurements in the mail box
LuisRA 0:2b691d200d6f 108 Returns: 0 if found data, -1 if not data available.
LuisRA 0:2b691d200d6f 109 */
LuisRA 0:2b691d200d6f 110 int32_t RPLidar::pollSensorData(struct RPLidarMeasurement *_data)
LuisRA 0:2b691d200d6f 111 {
LuisRA 0:2b691d200d6f 112 osEvent evt = mail_box.get();
LuisRA 0:2b691d200d6f 113 if (evt.status == osEventMail) {
LuisRA 0:2b691d200d6f 114
LuisRA 0:2b691d200d6f 115 struct RPLidarMeasurement *mail = (struct RPLidarMeasurement*)evt.value.p;
LuisRA 0:2b691d200d6f 116 //struct RPLidarMeasurement data;
LuisRA 0:2b691d200d6f 117 _data->distance = mail->distance;
LuisRA 0:2b691d200d6f 118 _data->angle = mail->angle;
LuisRA 0:2b691d200d6f 119 _data->quality = mail->quality;
LuisRA 0:2b691d200d6f 120 _data->startBit = mail->startBit;
LuisRA 0:2b691d200d6f 121 mail_box.free(mail);
LuisRA 0:2b691d200d6f 122 return 0;
LuisRA 0:2b691d200d6f 123
LuisRA 0:2b691d200d6f 124
LuisRA 0:2b691d200d6f 125
LuisRA 0:2b691d200d6f 126 }
LuisRA 0:2b691d200d6f 127
LuisRA 0:2b691d200d6f 128 return -1;
LuisRA 0:2b691d200d6f 129 }
LuisRA 0:2b691d200d6f 130
LuisRA 0:2b691d200d6f 131
LuisRA 0:2b691d200d6f 132 RPLidar::RPLidar()
LuisRA 0:2b691d200d6f 133 {
LuisRA 0:2b691d200d6f 134
LuisRA 0:2b691d200d6f 135
LuisRA 0:2b691d200d6f 136 _currentMeasurement.distance = 0;
LuisRA 0:2b691d200d6f 137 _currentMeasurement.angle = 0;
LuisRA 0:2b691d200d6f 138 _currentMeasurement.quality = 0;
LuisRA 0:2b691d200d6f 139 _currentMeasurement.startBit = 0;
LuisRA 0:2b691d200d6f 140
LuisRA 0:2b691d200d6f 141 useThread = 0;
LuisRA 0:2b691d200d6f 142 thread.set_priority(osPriorityAboveNormal);
LuisRA 0:2b691d200d6f 143 //Thread thread(osPriorityAboveNormal, OS_STACK_SIZE, NULL, "RPLidar");
LuisRA 0:2b691d200d6f 144 //thread_p = &thread;
LuisRA 0:2b691d200d6f 145 }
LuisRA 0:2b691d200d6f 146
LuisRA 0:2b691d200d6f 147
LuisRA 0:2b691d200d6f 148 RPLidar::~RPLidar()
LuisRA 0:2b691d200d6f 149 {
LuisRA 0:2b691d200d6f 150 end();
LuisRA 0:2b691d200d6f 151 }
LuisRA 0:2b691d200d6f 152
LuisRA 0:2b691d200d6f 153 // open the given serial interface and try to connect to the RPLIDAR
LuisRA 0:2b691d200d6f 154 /*
LuisRA 0:2b691d200d6f 155 bool RPLidar::begin(BufferedSerial &serialobj)
LuisRA 0:2b691d200d6f 156 {
LuisRA 0:2b691d200d6f 157
LuisRA 0:2b691d200d6f 158 //Serial.begin(115200);
LuisRA 0:2b691d200d6f 159
LuisRA 0:2b691d200d6f 160 if (isOpen()) {
LuisRA 0:2b691d200d6f 161 end();
LuisRA 0:2b691d200d6f 162 }
LuisRA 0:2b691d200d6f 163 _bined_serialdev = &serialobj;
LuisRA 0:2b691d200d6f 164 // _bined_serialdev->end();
LuisRA 0:2b691d200d6f 165 _bined_serialdev->baud(RPLIDAR_SERIAL_BAUDRATE);
LuisRA 0:2b691d200d6f 166 }
LuisRA 0:2b691d200d6f 167 */
LuisRA 0:2b691d200d6f 168
LuisRA 0:2b691d200d6f 169 void RPLidar::begin(BufferedSerial &serialobj)
LuisRA 0:2b691d200d6f 170 {
LuisRA 0:2b691d200d6f 171 _bined_serialdev = &serialobj;
LuisRA 0:2b691d200d6f 172 timers.start();
LuisRA 0:2b691d200d6f 173 _bined_serialdev->baud(RPLIDAR_SERIAL_BAUDRATE);
LuisRA 0:2b691d200d6f 174 recvPos_g = 0;
LuisRA 0:2b691d200d6f 175 }
LuisRA 0:2b691d200d6f 176 // close the currently opened serial interface
LuisRA 0:2b691d200d6f 177 void RPLidar::end()
LuisRA 0:2b691d200d6f 178 {/*
LuisRA 0:2b691d200d6f 179 if (isOpen()) {
LuisRA 0:2b691d200d6f 180 _bined_serialdev->end();
LuisRA 0:2b691d200d6f 181 _bined_serialdev = NULL;
LuisRA 0:2b691d200d6f 182 }*/
LuisRA 0:2b691d200d6f 183 }
LuisRA 0:2b691d200d6f 184
LuisRA 0:2b691d200d6f 185
LuisRA 0:2b691d200d6f 186 // check whether the serial interface is opened
LuisRA 0:2b691d200d6f 187 /*
LuisRA 0:2b691d200d6f 188 bool RPLidar::isOpen()
LuisRA 0:2b691d200d6f 189 {
LuisRA 0:2b691d200d6f 190 return _bined_serialdev?true:false;
LuisRA 0:2b691d200d6f 191 }
LuisRA 0:2b691d200d6f 192 */
LuisRA 0:2b691d200d6f 193 // ask the RPLIDAR for its health info
LuisRA 0:2b691d200d6f 194
LuisRA 0:2b691d200d6f 195 u_result RPLidar::getHealth(rplidar_response_device_health_t & healthinfo, _u32 timeout)
LuisRA 0:2b691d200d6f 196 {
LuisRA 0:2b691d200d6f 197 _u32 currentTs = timers.read_ms();
LuisRA 0:2b691d200d6f 198 _u32 remainingtime;
LuisRA 0:2b691d200d6f 199
LuisRA 0:2b691d200d6f 200 _u8 *infobuf = (_u8 *)&healthinfo;
LuisRA 0:2b691d200d6f 201 _u8 recvPos = 0;
LuisRA 0:2b691d200d6f 202
LuisRA 0:2b691d200d6f 203 rplidar_ans_header_t response_header;
LuisRA 0:2b691d200d6f 204 u_result ans;
LuisRA 0:2b691d200d6f 205
LuisRA 0:2b691d200d6f 206
LuisRA 0:2b691d200d6f 207 // if (!isOpen()) return RESULT_OPERATION_FAIL;
LuisRA 0:2b691d200d6f 208
LuisRA 0:2b691d200d6f 209 {
LuisRA 0:2b691d200d6f 210 if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_HEALTH, NULL, 0))) {
LuisRA 0:2b691d200d6f 211 return ans;
LuisRA 0:2b691d200d6f 212 }
LuisRA 0:2b691d200d6f 213
LuisRA 0:2b691d200d6f 214 if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
LuisRA 0:2b691d200d6f 215 return ans;
LuisRA 0:2b691d200d6f 216 }
LuisRA 0:2b691d200d6f 217
LuisRA 0:2b691d200d6f 218 // verify whether we got a correct header
LuisRA 0:2b691d200d6f 219 if (response_header.type != RPLIDAR_ANS_TYPE_DEVHEALTH) {
LuisRA 0:2b691d200d6f 220 return RESULT_INVALID_DATA;
LuisRA 0:2b691d200d6f 221 }
LuisRA 0:2b691d200d6f 222
LuisRA 0:2b691d200d6f 223 if ((response_header.size) < sizeof(rplidar_response_device_health_t)) {
LuisRA 0:2b691d200d6f 224 return RESULT_INVALID_DATA;
LuisRA 0:2b691d200d6f 225 }
LuisRA 0:2b691d200d6f 226
LuisRA 0:2b691d200d6f 227 while ((remainingtime=timers.read_ms() - currentTs) <= timeout) {
LuisRA 0:2b691d200d6f 228 int currentbyte = _bined_serialdev->getc();
LuisRA 0:2b691d200d6f 229 if (currentbyte < 0) continue;
LuisRA 0:2b691d200d6f 230
LuisRA 0:2b691d200d6f 231 infobuf[recvPos++] = currentbyte;
LuisRA 0:2b691d200d6f 232
LuisRA 0:2b691d200d6f 233 if (recvPos == sizeof(rplidar_response_device_health_t)) {
LuisRA 0:2b691d200d6f 234 return RESULT_OK;
LuisRA 0:2b691d200d6f 235 }
LuisRA 0:2b691d200d6f 236 }
LuisRA 0:2b691d200d6f 237 }
LuisRA 0:2b691d200d6f 238 return RESULT_OPERATION_TIMEOUT;
LuisRA 0:2b691d200d6f 239 }
LuisRA 0:2b691d200d6f 240 // ask the RPLIDAR for its device info like the serial number
LuisRA 0:2b691d200d6f 241 u_result RPLidar::getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout )
LuisRA 0:2b691d200d6f 242 {
LuisRA 0:2b691d200d6f 243 _u8 recvPos = 0;
LuisRA 0:2b691d200d6f 244 _u32 currentTs = timers.read_ms();
LuisRA 0:2b691d200d6f 245 _u32 remainingtime;
LuisRA 0:2b691d200d6f 246 _u8 *infobuf = (_u8*)&info;
LuisRA 0:2b691d200d6f 247 rplidar_ans_header_t response_header;
LuisRA 0:2b691d200d6f 248 u_result ans;
LuisRA 0:2b691d200d6f 249
LuisRA 0:2b691d200d6f 250 // if (!isOpen()) return RESULT_OPERATION_FAIL;
LuisRA 0:2b691d200d6f 251
LuisRA 0:2b691d200d6f 252 {
LuisRA 0:2b691d200d6f 253 if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_INFO,NULL,0))) {
LuisRA 0:2b691d200d6f 254 return ans;
LuisRA 0:2b691d200d6f 255 }
LuisRA 0:2b691d200d6f 256
LuisRA 0:2b691d200d6f 257 if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
LuisRA 0:2b691d200d6f 258 return ans;
LuisRA 0:2b691d200d6f 259 }
LuisRA 0:2b691d200d6f 260
LuisRA 0:2b691d200d6f 261 // verify whether we got a correct header
LuisRA 0:2b691d200d6f 262 if (response_header.type != RPLIDAR_ANS_TYPE_DEVINFO) {
LuisRA 0:2b691d200d6f 263 return RESULT_INVALID_DATA;
LuisRA 0:2b691d200d6f 264 }
LuisRA 0:2b691d200d6f 265
LuisRA 0:2b691d200d6f 266 if (response_header.size < sizeof(rplidar_response_device_info_t)) {
LuisRA 0:2b691d200d6f 267 return RESULT_INVALID_DATA;
LuisRA 0:2b691d200d6f 268 }
LuisRA 0:2b691d200d6f 269
LuisRA 0:2b691d200d6f 270 while ((remainingtime=timers.read_ms() - currentTs) <= timeout) {
LuisRA 0:2b691d200d6f 271 int currentbyte = _bined_serialdev->getc();
LuisRA 0:2b691d200d6f 272 if (currentbyte<0) continue;
LuisRA 0:2b691d200d6f 273 infobuf[recvPos++] = currentbyte;
LuisRA 0:2b691d200d6f 274
LuisRA 0:2b691d200d6f 275 if (recvPos == sizeof(rplidar_response_device_info_t)) {
LuisRA 0:2b691d200d6f 276 return RESULT_OK;
LuisRA 0:2b691d200d6f 277 }
LuisRA 0:2b691d200d6f 278 }
LuisRA 0:2b691d200d6f 279 }
LuisRA 0:2b691d200d6f 280
LuisRA 0:2b691d200d6f 281 return RESULT_OPERATION_TIMEOUT;
LuisRA 0:2b691d200d6f 282 }
LuisRA 0:2b691d200d6f 283
LuisRA 0:2b691d200d6f 284 // stop the measurement operation
LuisRA 0:2b691d200d6f 285 u_result RPLidar::stop()
LuisRA 0:2b691d200d6f 286 {
LuisRA 0:2b691d200d6f 287 // if (!isOpen()) return RESULT_OPERATION_FAIL;
LuisRA 0:2b691d200d6f 288 u_result ans = _sendCommand(RPLIDAR_CMD_STOP,NULL,0);
LuisRA 0:2b691d200d6f 289 return ans;
LuisRA 0:2b691d200d6f 290 }
LuisRA 0:2b691d200d6f 291
LuisRA 0:2b691d200d6f 292
LuisRA 0:2b691d200d6f 293
LuisRA 0:2b691d200d6f 294 u_result RPLidar::startThreadScan()
LuisRA 0:2b691d200d6f 295 {
LuisRA 0:2b691d200d6f 296 startScan();
LuisRA 0:2b691d200d6f 297 useThread = true;
LuisRA 0:2b691d200d6f 298 thread.start(callback(this, &RPLidar::Thread_readSensor));
LuisRA 0:2b691d200d6f 299
LuisRA 0:2b691d200d6f 300 return RESULT_OK;
LuisRA 0:2b691d200d6f 301 }
LuisRA 0:2b691d200d6f 302 // start the measurement operation
LuisRA 0:2b691d200d6f 303 /*
LuisRA 0:2b691d200d6f 304 This was altered to also start the thread that parses measurements in the background
LuisRA 0:2b691d200d6f 305 */
LuisRA 0:2b691d200d6f 306 u_result RPLidar::startScan(bool force, _u32 timeout)
LuisRA 0:2b691d200d6f 307 {
LuisRA 0:2b691d200d6f 308 //pc->printf("RPLidar::startScan\n");
LuisRA 0:2b691d200d6f 309 u_result ans;
LuisRA 0:2b691d200d6f 310
LuisRA 0:2b691d200d6f 311 // if (!isOpen()) return RESULT_OPERATION_FAIL;
LuisRA 0:2b691d200d6f 312
LuisRA 0:2b691d200d6f 313 stop(); //force the previous operation to stop
LuisRA 0:2b691d200d6f 314
LuisRA 0:2b691d200d6f 315
LuisRA 0:2b691d200d6f 316 ans = _sendCommand(force?RPLIDAR_CMD_FORCE_SCAN:RPLIDAR_CMD_SCAN, NULL, 0);
LuisRA 0:2b691d200d6f 317 if (IS_FAIL(ans)) return ans;
LuisRA 0:2b691d200d6f 318
LuisRA 0:2b691d200d6f 319 // waiting for confirmation
LuisRA 0:2b691d200d6f 320 rplidar_ans_header_t response_header;
LuisRA 0:2b691d200d6f 321 if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
LuisRA 0:2b691d200d6f 322 return ans;
LuisRA 0:2b691d200d6f 323 }
LuisRA 0:2b691d200d6f 324
LuisRA 0:2b691d200d6f 325 // verify whether we got a correct header
LuisRA 0:2b691d200d6f 326 if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT) {
LuisRA 0:2b691d200d6f 327 return RESULT_INVALID_DATA;
LuisRA 0:2b691d200d6f 328 }
LuisRA 0:2b691d200d6f 329
LuisRA 0:2b691d200d6f 330 if (response_header.size < sizeof(rplidar_response_measurement_node_t)) {
LuisRA 0:2b691d200d6f 331 return RESULT_INVALID_DATA;
LuisRA 0:2b691d200d6f 332 }
LuisRA 0:2b691d200d6f 333
LuisRA 0:2b691d200d6f 334
LuisRA 0:2b691d200d6f 335 return RESULT_OK;
LuisRA 0:2b691d200d6f 336 }
LuisRA 0:2b691d200d6f 337
LuisRA 0:2b691d200d6f 338 // wait for one sample point to arrive
LuisRA 0:2b691d200d6f 339 /*
LuisRA 0:2b691d200d6f 340 Do not use if startScan was called with starThread == 1!
LuisRA 0:2b691d200d6f 341 */
LuisRA 0:2b691d200d6f 342 u_result RPLidar::waitPoint(_u32 timeout)
LuisRA 0:2b691d200d6f 343 {
LuisRA 0:2b691d200d6f 344
LuisRA 0:2b691d200d6f 345 if(useThread)
LuisRA 0:2b691d200d6f 346 return RESULT_OPERATION_NOT_SUPPORT;
LuisRA 0:2b691d200d6f 347
LuisRA 0:2b691d200d6f 348 _u32 currentTs = timers.read_ms();
LuisRA 0:2b691d200d6f 349 _u32 remainingtime;
LuisRA 0:2b691d200d6f 350 rplidar_response_measurement_node_t node;
LuisRA 0:2b691d200d6f 351 _u8 *nodebuf = (_u8*)&node;
LuisRA 0:2b691d200d6f 352
LuisRA 0:2b691d200d6f 353 _u8 recvPos = 0;
LuisRA 0:2b691d200d6f 354
LuisRA 0:2b691d200d6f 355 while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
LuisRA 0:2b691d200d6f 356 int currentbyte = _bined_serialdev->getc();
LuisRA 0:2b691d200d6f 357 if (currentbyte<0) continue;
LuisRA 0:2b691d200d6f 358 //Serial.println(currentbyte);
LuisRA 0:2b691d200d6f 359 switch (recvPos) {
LuisRA 0:2b691d200d6f 360 case 0: // expect the sync bit and its reverse in this byte {
LuisRA 0:2b691d200d6f 361 {
LuisRA 0:2b691d200d6f 362 _u8 tmp = (currentbyte>>1);
LuisRA 0:2b691d200d6f 363 if ( (tmp ^ currentbyte) & 0x1 ) {
LuisRA 0:2b691d200d6f 364 // pass
LuisRA 0:2b691d200d6f 365 } else {
LuisRA 0:2b691d200d6f 366 continue;
LuisRA 0:2b691d200d6f 367 }
LuisRA 0:2b691d200d6f 368
LuisRA 0:2b691d200d6f 369 }
LuisRA 0:2b691d200d6f 370 break;
LuisRA 0:2b691d200d6f 371 case 1: // expect the highest bit to be 1
LuisRA 0:2b691d200d6f 372 {
LuisRA 0:2b691d200d6f 373 if (currentbyte & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) {
LuisRA 0:2b691d200d6f 374 // pass
LuisRA 0:2b691d200d6f 375 } else {
LuisRA 0:2b691d200d6f 376 recvPos = 0;
LuisRA 0:2b691d200d6f 377 continue;
LuisRA 0:2b691d200d6f 378 }
LuisRA 0:2b691d200d6f 379 }
LuisRA 0:2b691d200d6f 380 break;
LuisRA 0:2b691d200d6f 381 }
LuisRA 0:2b691d200d6f 382 nodebuf[recvPos++] = currentbyte;
LuisRA 0:2b691d200d6f 383 if (recvPos == sizeof(rplidar_response_measurement_node_t)) {
LuisRA 0:2b691d200d6f 384 // store the data ...
LuisRA 0:2b691d200d6f 385 _currentMeasurement.distance = node.distance_q2/4.0f;
LuisRA 0:2b691d200d6f 386 _currentMeasurement.angle = (node.angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
LuisRA 0:2b691d200d6f 387 _currentMeasurement.quality = (node.sync_quality>>RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
LuisRA 0:2b691d200d6f 388 _currentMeasurement.startBit = (node.sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT);
LuisRA 0:2b691d200d6f 389 ang = _currentMeasurement.angle;
LuisRA 0:2b691d200d6f 390 dis = _currentMeasurement.distance;
LuisRA 0:2b691d200d6f 391
LuisRA 0:2b691d200d6f 392
LuisRA 0:2b691d200d6f 393 if(ang>=angMin&&ang<=angMax)Data[ang] = dis;
LuisRA 0:2b691d200d6f 394
LuisRA 0:2b691d200d6f 395 return RESULT_OK;
LuisRA 0:2b691d200d6f 396 }
LuisRA 0:2b691d200d6f 397
LuisRA 0:2b691d200d6f 398
LuisRA 0:2b691d200d6f 399 }
LuisRA 0:2b691d200d6f 400
LuisRA 0:2b691d200d6f 401 return RESULT_OPERATION_TIMEOUT;
LuisRA 0:2b691d200d6f 402 }
LuisRA 0:2b691d200d6f 403
LuisRA 0:2b691d200d6f 404
LuisRA 0:2b691d200d6f 405 /*
LuisRA 0:2b691d200d6f 406 This is very similar to waitPoint and it's only to be used by the thread,
LuisRA 0:2b691d200d6f 407 hence why it private.
LuisRA 0:2b691d200d6f 408
LuisRA 0:2b691d200d6f 409 It checks for data in the buffered serial until it finds a message or there
LuisRA 0:2b691d200d6f 410 are no more bytes.
LuisRA 0:2b691d200d6f 411 If it finds a message it returns RESULT_OK
LuisRA 0:2b691d200d6f 412 If there are no more bytes it returns RESULT_OPERATION_TIMEOUT.
LuisRA 0:2b691d200d6f 413
LuisRA 0:2b691d200d6f 414 The state of the parsing is saved so it can continue parsing a measurement
LuisRA 0:2b691d200d6f 415 midway. (Note that does not mean it's re-entrant and should only be used
LuisRA 0:2b691d200d6f 416 in the same context)
LuisRA 0:2b691d200d6f 417 */
LuisRA 0:2b691d200d6f 418 u_result RPLidar::pollPoint()
LuisRA 0:2b691d200d6f 419 {
LuisRA 0:2b691d200d6f 420
LuisRA 0:2b691d200d6f 421 //_u32 currentTs = timers.read_ms();
LuisRA 0:2b691d200d6f 422 _u32 remainingtime;
LuisRA 0:2b691d200d6f 423 //rplidar_response_measurement_node_t node;
LuisRA 0:2b691d200d6f 424 _u8 *nodebuf = (_u8*)&(node_g);
LuisRA 0:2b691d200d6f 425
LuisRA 0:2b691d200d6f 426
LuisRA 0:2b691d200d6f 427 int currentbyte;
LuisRA 0:2b691d200d6f 428 while(_bined_serialdev->readable() > 0)
LuisRA 0:2b691d200d6f 429 {
LuisRA 0:2b691d200d6f 430 //while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
LuisRA 0:2b691d200d6f 431 currentbyte = _bined_serialdev->getc();
LuisRA 0:2b691d200d6f 432 //if (_bined_serialdev->readable() == 0) continue;
LuisRA 0:2b691d200d6f 433 //Serial.println(currentbyte);
LuisRA 0:2b691d200d6f 434 switch (recvPos_g) {
LuisRA 0:2b691d200d6f 435 case 0: // expect the sync bit and its reverse in this byte {
LuisRA 0:2b691d200d6f 436 {
LuisRA 0:2b691d200d6f 437 _u8 tmp = (currentbyte>>1);
LuisRA 0:2b691d200d6f 438 if ( (tmp ^ currentbyte) & 0x1 ) {
LuisRA 0:2b691d200d6f 439 // pass
LuisRA 0:2b691d200d6f 440
LuisRA 0:2b691d200d6f 441 } else {
LuisRA 0:2b691d200d6f 442 continue;
LuisRA 0:2b691d200d6f 443 }
LuisRA 0:2b691d200d6f 444
LuisRA 0:2b691d200d6f 445 }
LuisRA 0:2b691d200d6f 446 break;
LuisRA 0:2b691d200d6f 447 case 1: // expect the highest bit to be 1
LuisRA 0:2b691d200d6f 448 {
LuisRA 0:2b691d200d6f 449 if (currentbyte & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) {
LuisRA 0:2b691d200d6f 450 // pass
LuisRA 0:2b691d200d6f 451 } else {
LuisRA 0:2b691d200d6f 452 recvPos_g = 0;
LuisRA 0:2b691d200d6f 453 continue;
LuisRA 0:2b691d200d6f 454 }
LuisRA 0:2b691d200d6f 455 }
LuisRA 0:2b691d200d6f 456 break;
LuisRA 0:2b691d200d6f 457 }
LuisRA 0:2b691d200d6f 458 nodebuf[recvPos_g++] = currentbyte;
LuisRA 0:2b691d200d6f 459 if (recvPos_g == sizeof(rplidar_response_measurement_node_t)) {
LuisRA 0:2b691d200d6f 460 recvPos_g = 0;
LuisRA 0:2b691d200d6f 461 // store the data ...
LuisRA 0:2b691d200d6f 462 _currentMeasurement.distance = node_g.distance_q2/4.0f;
LuisRA 0:2b691d200d6f 463 _currentMeasurement.angle = (node_g.angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
LuisRA 0:2b691d200d6f 464 _currentMeasurement.quality = (node_g.sync_quality>>RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
LuisRA 0:2b691d200d6f 465 _currentMeasurement.startBit = (node_g.sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT);
LuisRA 0:2b691d200d6f 466 ang = _currentMeasurement.angle;
LuisRA 0:2b691d200d6f 467 dis = _currentMeasurement.distance;
LuisRA 0:2b691d200d6f 468
LuisRA 0:2b691d200d6f 469
LuisRA 0:2b691d200d6f 470 if(ang>=angMin&&ang<=angMax)Data[ang] = dis;
LuisRA 0:2b691d200d6f 471
LuisRA 0:2b691d200d6f 472 return RESULT_OK;
LuisRA 0:2b691d200d6f 473 }
LuisRA 0:2b691d200d6f 474
LuisRA 0:2b691d200d6f 475
LuisRA 0:2b691d200d6f 476 }//while(_bined_serialdev->readable() > 0);
LuisRA 0:2b691d200d6f 477
LuisRA 0:2b691d200d6f 478 return RESULT_OPERATION_TIMEOUT;
LuisRA 0:2b691d200d6f 479 }
LuisRA 0:2b691d200d6f 480
LuisRA 0:2b691d200d6f 481
LuisRA 0:2b691d200d6f 482 void RPLidar::setAngle(int min,int max){
LuisRA 0:2b691d200d6f 483 angMin = min;
LuisRA 0:2b691d200d6f 484 angMax = max;
LuisRA 0:2b691d200d6f 485 }
LuisRA 0:2b691d200d6f 486 void RPLidar::get_lidar(){
LuisRA 0:2b691d200d6f 487 if (!IS_OK(waitPoint())) startScan();
LuisRA 0:2b691d200d6f 488 }
LuisRA 0:2b691d200d6f 489 u_result RPLidar::_sendCommand(_u8 cmd, const void * payload, size_t payloadsize)
LuisRA 0:2b691d200d6f 490 {
LuisRA 0:2b691d200d6f 491
LuisRA 0:2b691d200d6f 492 rplidar_cmd_packet_t pkt_header;
LuisRA 0:2b691d200d6f 493 rplidar_cmd_packet_t * header = &pkt_header;
LuisRA 0:2b691d200d6f 494 _u8 checksum = 0;
LuisRA 0:2b691d200d6f 495
LuisRA 0:2b691d200d6f 496 if (payloadsize && payload) {
LuisRA 0:2b691d200d6f 497 cmd |= RPLIDAR_CMDFLAG_HAS_PAYLOAD;
LuisRA 0:2b691d200d6f 498 }
LuisRA 0:2b691d200d6f 499
LuisRA 0:2b691d200d6f 500 header->syncByte = RPLIDAR_CMD_SYNC_BYTE;
LuisRA 0:2b691d200d6f 501 header->cmd_flag = cmd;
LuisRA 0:2b691d200d6f 502
LuisRA 0:2b691d200d6f 503 // send header first
LuisRA 0:2b691d200d6f 504 _bined_serialdev->putc(header->syncByte );
LuisRA 0:2b691d200d6f 505 _bined_serialdev->putc(header->cmd_flag );
LuisRA 0:2b691d200d6f 506
LuisRA 0:2b691d200d6f 507 // _bined_serialdev->write( (uint8_t *)header, 2);
LuisRA 0:2b691d200d6f 508
LuisRA 0:2b691d200d6f 509 if (cmd & RPLIDAR_CMDFLAG_HAS_PAYLOAD) {
LuisRA 0:2b691d200d6f 510 checksum ^= RPLIDAR_CMD_SYNC_BYTE;
LuisRA 0:2b691d200d6f 511 checksum ^= cmd;
LuisRA 0:2b691d200d6f 512 checksum ^= (payloadsize & 0xFF);
LuisRA 0:2b691d200d6f 513
LuisRA 0:2b691d200d6f 514 // calc checksum
LuisRA 0:2b691d200d6f 515 for (size_t pos = 0; pos < payloadsize; ++pos) {
LuisRA 0:2b691d200d6f 516 checksum ^= ((_u8 *)payload)[pos];
LuisRA 0:2b691d200d6f 517 }
LuisRA 0:2b691d200d6f 518
LuisRA 0:2b691d200d6f 519 // send size
LuisRA 0:2b691d200d6f 520 _u8 sizebyte = payloadsize;
LuisRA 0:2b691d200d6f 521 _bined_serialdev->putc(sizebyte);
LuisRA 0:2b691d200d6f 522 // _bined_serialdev->write((uint8_t *)&sizebyte, 1);
LuisRA 0:2b691d200d6f 523
LuisRA 0:2b691d200d6f 524 // send payload
LuisRA 0:2b691d200d6f 525 // _bined_serialdev.putc((uint8_t )payload);
LuisRA 0:2b691d200d6f 526 // _bined_serialdev->write((uint8_t *)&payload, sizebyte);
LuisRA 0:2b691d200d6f 527
LuisRA 0:2b691d200d6f 528 // send checksum
LuisRA 0:2b691d200d6f 529 _bined_serialdev->putc(checksum);
LuisRA 0:2b691d200d6f 530 // _bined_serialdev->write((uint8_t *)&checksum, 1);
LuisRA 0:2b691d200d6f 531
LuisRA 0:2b691d200d6f 532 }
LuisRA 0:2b691d200d6f 533
LuisRA 0:2b691d200d6f 534 return RESULT_OK;
LuisRA 0:2b691d200d6f 535 }
LuisRA 0:2b691d200d6f 536
LuisRA 0:2b691d200d6f 537 u_result RPLidar::_waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout)
LuisRA 0:2b691d200d6f 538 {
LuisRA 0:2b691d200d6f 539 _u8 recvPos = 0;
LuisRA 0:2b691d200d6f 540 _u32 currentTs = timers.read_ms();
LuisRA 0:2b691d200d6f 541 _u32 remainingtime;
LuisRA 0:2b691d200d6f 542 _u8 *headerbuf = (_u8*)header;
LuisRA 0:2b691d200d6f 543 while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
LuisRA 0:2b691d200d6f 544
LuisRA 0:2b691d200d6f 545 int currentbyte = _bined_serialdev->getc();
LuisRA 0:2b691d200d6f 546 if (currentbyte<0) continue;
LuisRA 0:2b691d200d6f 547 switch (recvPos) {
LuisRA 0:2b691d200d6f 548 case 0:
LuisRA 0:2b691d200d6f 549 if (currentbyte != RPLIDAR_ANS_SYNC_BYTE1) {
LuisRA 0:2b691d200d6f 550 continue;
LuisRA 0:2b691d200d6f 551 }
LuisRA 0:2b691d200d6f 552 break;
LuisRA 0:2b691d200d6f 553 case 1:
LuisRA 0:2b691d200d6f 554 if (currentbyte != RPLIDAR_ANS_SYNC_BYTE2) {
LuisRA 0:2b691d200d6f 555 recvPos = 0;
LuisRA 0:2b691d200d6f 556 continue;
LuisRA 0:2b691d200d6f 557 }
LuisRA 0:2b691d200d6f 558 break;
LuisRA 0:2b691d200d6f 559 }
LuisRA 0:2b691d200d6f 560 headerbuf[recvPos++] = currentbyte;
LuisRA 0:2b691d200d6f 561
LuisRA 0:2b691d200d6f 562 if (recvPos == sizeof(rplidar_ans_header_t)) {
LuisRA 0:2b691d200d6f 563 return RESULT_OK;
LuisRA 0:2b691d200d6f 564 }
LuisRA 0:2b691d200d6f 565
LuisRA 0:2b691d200d6f 566
LuisRA 0:2b691d200d6f 567 }
LuisRA 0:2b691d200d6f 568
LuisRA 0:2b691d200d6f 569 return RESULT_OPERATION_TIMEOUT;
LuisRA 0:2b691d200d6f 570 }