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?
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 | |
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 | } |