Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : /*
17 : * ArduPilot device driver for SLAMTEC RPLIDAR A2 (16m range version)
18 : *
19 : * ALL INFORMATION REGARDING PROTOCOL WAS DERIVED FROM RPLIDAR DATASHEET:
20 : *
21 : * https://www.slamtec.com/en/Lidar
22 : * http://bucket.download.slamtec.com/63ac3f0d8c859d3a10e51c6b3285fcce25a47357/LR001_SLAMTEC_rplidar_protocol_v1.0_en.pdf
23 : *
24 : * Author: Steven Josefs, IAV GmbH
25 : * Based on the LightWare SF40C ArduPilot device driver from Randy Mackay
26 : *
27 : */
28 :
29 : #include "AP_Proximity_config.h"
30 :
31 : #if AP_PROXIMITY_RPLIDARA2_ENABLED
32 :
33 : #include "AP_Proximity_RPLidarA2.h"
34 :
35 : #include <AP_HAL/AP_HAL.h>
36 : #include <AP_InternalError/AP_InternalError.h>
37 :
38 : #include <ctype.h>
39 : #include <stdio.h>
40 :
41 : #define RP_DEBUG_LEVEL 0
42 :
43 : #include <GCS_MAVLink/GCS.h>
44 : #if RP_DEBUG_LEVEL
45 : #define Debug(level, fmt, args ...) do { if (level <= RP_DEBUG_LEVEL) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, fmt, ## args); } } while (0)
46 : #else
47 : #define Debug(level, fmt, args ...)
48 : #endif
49 :
50 : #define COMM_ACTIVITY_TIMEOUT_MS 200
51 :
52 : // Commands
53 : //-----------------------------------------
54 :
55 : // Commands without payload and response
56 : #define RPLIDAR_PREAMBLE 0xA5
57 : #define RPLIDAR_CMD_STOP 0x25
58 : #define RPLIDAR_CMD_SCAN 0x20
59 : #define RPLIDAR_CMD_FORCE_SCAN 0x21
60 : #define RPLIDAR_CMD_RESET 0x40
61 :
62 : // Commands without payload but have response
63 : #define RPLIDAR_CMD_GET_DEVICE_INFO 0x50
64 : #define RPLIDAR_CMD_GET_DEVICE_HEALTH 0x52
65 :
66 : #if AP_PROXIMITY_RPLIDAR_EXPRESSSCAN_ENABLED
67 : // Commands with payload and have response
68 : #define RPLIDAR_CMD_EXPRESS_SCAN 0x82
69 : #endif
70 :
71 : extern const AP_HAL::HAL& hal;
72 :
73 24819 : void AP_Proximity_RPLidarA2::update(void)
74 : {
75 24819 : if (_uart == nullptr) {
76 0 : return;
77 : }
78 :
79 : // request device info 3sec after reset
80 : // required for S1 support that sends only 9 bytes after a reset (A1,A2 send 63)
81 24819 : uint32_t now_ms = AP_HAL::millis();
82 24819 : if ((_state == State::RESET) && (now_ms - _last_reset_ms > 3000)) {
83 3 : send_request_for_device_info();
84 3 : _state = State::AWAITING_RESPONSE;
85 3 : _byte_count = 0;
86 : }
87 :
88 24819 : get_readings();
89 :
90 : // check for timeout and set health status
91 24819 : if (AP_HAL::millis() - _last_distance_received_ms > COMM_ACTIVITY_TIMEOUT_MS) {
92 344 : set_status(AP_Proximity::Status::NoData);
93 : Debug(1, "LIDAR NO DATA");
94 344 : if (AP_HAL::millis() - _last_reset_ms > 10000) {
95 0 : reset_rplidar();
96 : }
97 : } else {
98 24475 : set_status(AP_Proximity::Status::Good);
99 : }
100 : }
101 :
102 : // get maximum distance (in meters) of sensor
103 7437 : float AP_Proximity_RPLidarA2::distance_max_m() const
104 : {
105 7437 : switch (model) {
106 84 : case Model::UNKNOWN:
107 84 : return 0.0f;
108 0 : case Model::A1:
109 0 : return 8.0f;
110 4910 : case Model::A2:
111 4910 : return 16.0f;
112 0 : case Model::A2M12:
113 : case Model::C1:
114 0 : return 12.0f;
115 0 : case Model::S1:
116 0 : return 40.0f;
117 2443 : case Model::S2:
118 2443 : return 50.0f;
119 : }
120 0 : return 0.0f;
121 : }
122 :
123 : // get minimum distance (in meters) of sensor
124 85736 : float AP_Proximity_RPLidarA2::distance_min_m() const
125 : {
126 85736 : switch (model) {
127 9 : case Model::UNKNOWN:
128 9 : return 0.0f;
129 82136 : case Model::A1:
130 : case Model::A2:
131 : case Model::A2M12:
132 : case Model::C1:
133 : case Model::S1:
134 82136 : return 0.2f;
135 3591 : case Model::S2:
136 3591 : return 0.05f;
137 : }
138 0 : return 0.0f;
139 : }
140 :
141 0 : void AP_Proximity_RPLidarA2::reset_rplidar()
142 : {
143 : static const uint8_t tx_buffer[2] {RPLIDAR_PREAMBLE, RPLIDAR_CMD_RESET};
144 0 : _uart->write(tx_buffer, 2);
145 : Debug(1, "LIDAR reset");
146 : // To-Do: ensure delay of 8m after sending reset request
147 0 : _last_reset_ms = AP_HAL::millis();
148 0 : reset();
149 0 : }
150 :
151 : // set Lidar into SCAN mode
152 2 : void AP_Proximity_RPLidarA2::send_scan_mode_request()
153 : {
154 : static const uint8_t tx_buffer[2] {RPLIDAR_PREAMBLE, RPLIDAR_CMD_SCAN};
155 2 : _uart->write(tx_buffer, 2);
156 : Debug(1, "Sent scan mode request");
157 2 : }
158 :
159 : #if AP_PROXIMITY_RPLIDAR_EXPRESSSCAN_ENABLED
160 : // send EXPRESS_SCAN request for Dense mode
161 1 : void AP_Proximity_RPLidarA2::send_express_scan_request()
162 : {
163 1 : const uint8_t cmd = RPLIDAR_CMD_EXPRESS_SCAN;
164 1 : const uint8_t payload_size = 5;
165 1 : uint8_t payload[payload_size] {};
166 :
167 1 : uint8_t checksum = 0;
168 1 : checksum ^= RPLIDAR_PREAMBLE;
169 1 : checksum ^= cmd;
170 1 : checksum ^= payload_size;
171 6 : for (uint8_t i = 0; i < sizeof(payload); i++) {
172 5 : checksum ^= payload[i];
173 : }
174 :
175 : // 2 (preamble + cmd) + 1 (payload_size field) + payload_size + 1 (checksum)
176 : uint8_t tx_buffer[2 + 1 + payload_size + 1];
177 1 : tx_buffer[0] = RPLIDAR_PREAMBLE;
178 1 : tx_buffer[1] = cmd;
179 1 : tx_buffer[2] = payload_size;
180 1 : memcpy(&tx_buffer[3], payload, sizeof(payload));
181 1 : tx_buffer[3 + sizeof(payload)] = checksum;
182 :
183 1 : _uart->write(tx_buffer, sizeof(tx_buffer));
184 : Debug(1, "Sent EXPRESS (Dense) scan request");
185 1 : }
186 : #endif // AP_PROXIMITY_RPLIDAR_EXPRESSSCAN_ENABLED
187 :
188 : // send request for sensor health
189 0 : void AP_Proximity_RPLidarA2::send_request_for_health() //not called yet
190 : {
191 : static const uint8_t tx_buffer[2] {RPLIDAR_PREAMBLE, RPLIDAR_CMD_GET_DEVICE_HEALTH};
192 0 : _uart->write(tx_buffer, 2);
193 : Debug(1, "Sent health request");
194 0 : }
195 :
196 : // send request for device information
197 3 : void AP_Proximity_RPLidarA2::send_request_for_device_info()
198 : {
199 : static const uint8_t tx_buffer[2] {RPLIDAR_PREAMBLE, RPLIDAR_CMD_GET_DEVICE_INFO};
200 3 : _uart->write(tx_buffer, 2);
201 : Debug(1, "Sent device information request");
202 3 : }
203 :
204 81734 : void AP_Proximity_RPLidarA2::consume_bytes(uint16_t count)
205 : {
206 81734 : if (count > _byte_count) {
207 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
208 0 : _byte_count = 0;
209 0 : return;
210 : }
211 81734 : _byte_count -= count;
212 81734 : if (_byte_count) {
213 65385 : memmove((void*)&_payload[0], (void*)&_payload[count], _byte_count);
214 : }
215 : }
216 :
217 0 : void AP_Proximity_RPLidarA2::reset()
218 : {
219 0 : _state = State::RESET;
220 0 : _byte_count = 0;
221 0 : _sync_error = 0;
222 : #if AP_PROXIMITY_RPLIDAR_EXPRESSSCAN_ENABLED
223 0 : _use_dense_express = false;
224 0 : _express_stream_len = 0;
225 : #endif
226 0 : }
227 :
228 0 : bool AP_Proximity_RPLidarA2::make_first_byte_in_payload(uint8_t desired_byte)
229 : {
230 0 : if (_byte_count == 0) {
231 0 : return false;
232 : }
233 0 : if (_payload[0] == desired_byte) {
234 0 : return true;
235 : }
236 0 : for (auto i=1; i<_byte_count; i++) {
237 0 : if (_payload[i] == desired_byte) {
238 0 : consume_bytes(i);
239 0 : return true;
240 : }
241 : }
242 : // just not in our buffer. Throw everything away:
243 0 : _byte_count = 0;
244 0 : return false;
245 : }
246 :
247 24819 : void AP_Proximity_RPLidarA2::get_readings()
248 : {
249 : Debug(2, " CURRENT STATE: %u ", (unsigned)_state);
250 :
251 : #if AP_PROXIMITY_RPLIDAR_EXPRESSSCAN_ENABLED
252 : // Express mode reads directly into _express_stream, bypassing _payload
253 24819 : if (_state == State::AWAITING_EXPRESS_DATA) {
254 8130 : handle_express_data();
255 8469 : return;
256 : }
257 : #endif
258 :
259 16689 : const uint32_t nbytes = _uart->available();
260 16689 : if (nbytes == 0) {
261 339 : return;
262 : }
263 16350 : const uint32_t bytes_to_read = MIN(nbytes, sizeof(_payload)-_byte_count);
264 16350 : if (bytes_to_read == 0) {
265 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
266 0 : reset();
267 0 : return;
268 : }
269 16350 : const uint32_t bytes_read = _uart->read(&_payload[_byte_count], bytes_to_read);
270 16350 : if (bytes_read == 0) {
271 : // this is bad; we were told there were bytes available
272 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
273 0 : reset();
274 0 : return;
275 : }
276 16350 : _byte_count += bytes_read;
277 :
278 16350 : uint32_t previous_loop_byte_count = UINT32_MAX;
279 98085 : while (_byte_count) {
280 81735 : if (_byte_count >= previous_loop_byte_count) {
281 : // this is a serious error, we should always consume some
282 : // bytes. Avoid looping forever.
283 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
284 0 : _uart = nullptr;
285 0 : return;
286 : }
287 81735 : previous_loop_byte_count = _byte_count;
288 :
289 81735 : switch(_state){
290 0 : case State::RESET: {
291 : // looking for 0x52 at start of buffer; the 62 following
292 : // bytes are "information"
293 0 : if (!make_first_byte_in_payload('R')) { // that's 'R' as in RPiLidar
294 0 : return;
295 : }
296 0 : if (_byte_count < 63) {
297 0 : return;
298 : }
299 : #if RP_DEBUG_LEVEL
300 : // optionally spit out via mavlink the 63-bytes of cruft
301 : // that is spat out on device reset
302 : Debug(1, "Got RPLidar Information");
303 : char xbuffer[64]{};
304 : memcpy((void*)xbuffer, (void*)&_payload.information, 63);
305 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "RPLidar: (%s)", xbuffer);
306 : #endif
307 : // 63 is the magic number of bytes in the spewed-out
308 : // reset data ... so now we'll just drop that stuff on
309 : // the floor.
310 0 : consume_bytes(63);
311 0 : send_request_for_device_info();
312 0 : _state = State::AWAITING_RESPONSE;
313 0 : continue;
314 : }
315 6 : case State::AWAITING_RESPONSE:
316 6 : if (_payload[0] != RPLIDAR_PREAMBLE) {
317 : // this is a protocol error. Reset.
318 0 : reset();
319 0 : return;
320 : }
321 :
322 : // descriptor packet has 7 byte in total
323 6 : if (_byte_count < sizeof(_descriptor)) {
324 0 : return;
325 : }
326 : // identify the payload data after the descriptor
327 : static const _descriptor SCAN_DATA_DESCRIPTOR[] {
328 : { RPLIDAR_PREAMBLE, 0x5A, 0x05, 0x00, 0x00, 0x40, 0x81 }
329 : };
330 : static const _descriptor HEALTH_DESCRIPTOR[] {
331 : { RPLIDAR_PREAMBLE, 0x5A, 0x03, 0x00, 0x00, 0x00, 0x06 }
332 : };
333 : static const _descriptor DEVICE_INFO_DESCRIPTOR[] {
334 : { RPLIDAR_PREAMBLE, 0x5A, 0x14, 0x00, 0x00, 0x00, 0x04 }
335 : };
336 : #if AP_PROXIMITY_RPLIDAR_EXPRESSSCAN_ENABLED
337 : static const _descriptor EXPRESS_DATA_DESCRIPTOR[] {
338 : { RPLIDAR_PREAMBLE, 0x5A, 0x54, 0x00, 0x00, 0x40, 0x85 }
339 : };
340 : #endif
341 : Debug(2,"LIDAR descriptor found");
342 6 : if (memcmp((void*)&_payload[0], SCAN_DATA_DESCRIPTOR, sizeof(_descriptor)) == 0) {
343 2 : _state = State::AWAITING_SCAN_DATA;
344 4 : } else if (memcmp((void*)&_payload[0], DEVICE_INFO_DESCRIPTOR, sizeof(_descriptor)) == 0) {
345 3 : _state = State::AWAITING_DEVICE_INFO;
346 1 : } else if (memcmp((void*)&_payload[0], HEALTH_DESCRIPTOR, sizeof(_descriptor)) == 0) {
347 0 : _state = State::AWAITING_HEALTH;
348 : #if AP_PROXIMITY_RPLIDAR_EXPRESSSCAN_ENABLED
349 1 : } else if (_use_dense_express && memcmp((void*)&_payload[0], EXPRESS_DATA_DESCRIPTOR, sizeof(_descriptor)) == 0) {
350 1 : _state = State::AWAITING_EXPRESS_DATA;
351 1 : _express_stream_len = 0;
352 1 : _sync_error = 0;
353 1 : const uint16_t express_bytes = _byte_count - sizeof(_descriptor);
354 1 : if (express_bytes != 0) {
355 0 : memcpy(_express_stream, &_payload[sizeof(_descriptor)], express_bytes);
356 0 : _express_stream_len = express_bytes;
357 : }
358 1 : _byte_count = 0;
359 1 : break;
360 : #endif
361 : } else {
362 : // unknown descriptor. Ignore it.
363 : }
364 5 : consume_bytes(sizeof(_descriptor));
365 5 : break;
366 :
367 3 : case State::AWAITING_DEVICE_INFO:
368 3 : if (_byte_count < sizeof(_payload.device_info)) {
369 0 : return;
370 : }
371 3 : parse_response_device_info();
372 3 : consume_bytes(sizeof(_payload.device_info));
373 3 : break;
374 :
375 81726 : case State::AWAITING_SCAN_DATA:
376 81726 : if (_byte_count < sizeof(_payload.sensor_scan)) {
377 0 : return;
378 : }
379 81726 : parse_response_data();
380 81726 : consume_bytes(sizeof(_payload.sensor_scan));
381 81726 : break;
382 :
383 0 : case State::AWAITING_HEALTH:
384 0 : if (_byte_count < sizeof(_payload.sensor_health)) {
385 0 : return;
386 : }
387 0 : parse_response_health();
388 0 : consume_bytes(sizeof(_payload.sensor_health));
389 0 : break;
390 :
391 : #if AP_PROXIMITY_RPLIDAR_EXPRESSSCAN_ENABLED
392 0 : case State::AWAITING_EXPRESS_DATA:
393 0 : break; // handled above, before _payload read
394 : #endif
395 0 : }
396 : }
397 : }
398 :
399 3 : void AP_Proximity_RPLidarA2::parse_response_device_info()
400 : {
401 : Debug(1, "Received DEVICE_INFO");
402 3 : const char *device_type = "UNKNOWN";
403 3 : switch (_payload.device_info.model) {
404 0 : case 0x18:
405 0 : model = Model::A1;
406 0 : device_type = "A1";
407 0 : break;
408 2 : case 0x28:
409 2 : model = Model::A2;
410 2 : device_type = "A2";
411 2 : break;
412 0 : case 0x2C:
413 0 : model = Model::A2M12;
414 0 : device_type = "A2M12";
415 0 : break;
416 0 : case 0x41:
417 0 : model=Model::C1;
418 0 : device_type="C1";
419 0 : break;
420 0 : case 0x61:
421 0 : model = Model::S1;
422 0 : device_type = "S1";
423 0 : break;
424 1 : case 0x71:
425 1 : model = Model::S2;
426 1 : device_type = "S2";
427 1 : break;
428 3 : default:
429 : Debug(1, "Unknown device (%u)", _payload.device_info.model);
430 : }
431 3 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "RPLidar %s hw=%u fw=%u.%u", device_type, _payload.device_info.hardware, _payload.device_info.firmware_minor, _payload.device_info.firmware_major);
432 :
433 : #if AP_PROXIMITY_RPLIDAR_EXPRESSSCAN_ENABLED
434 : // enable Dense EXPRESS path
435 3 : _use_dense_express = (model == Model::S2);
436 3 : if (_use_dense_express) {
437 1 : _express_stream_len = 0;
438 1 : send_express_scan_request();
439 : } else {
440 2 : send_scan_mode_request();
441 : }
442 : #else
443 : if (model == Model::S2) {
444 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RPLidar S2 ExpressScan disabled");
445 : return;
446 : }
447 : send_scan_mode_request();
448 : #endif
449 3 : _state = State::AWAITING_RESPONSE;
450 3 : }
451 :
452 81726 : void AP_Proximity_RPLidarA2::parse_response_data()
453 : {
454 81726 : if (_sync_error) {
455 : // out of 5-byte sync mask -> catch new revolution
456 : Debug(1, " OUT OF SYNC");
457 : // on first revolution bit 1 = 1, bit 2 = 0 of the first byte
458 0 : if ((_payload[0] & 0x03) == 0x01) {
459 0 : _sync_error = 0;
460 : Debug(1, " RESYNC");
461 : } else {
462 0 : return;
463 : }
464 : }
465 : Debug(2, "UART %02x %02x%02x %02x%02x", _payload[0], _payload[2], _payload[1], _payload[4], _payload[3]); //show HEX values
466 : // check if valid SCAN packet: a valid packet starts with startbits which are complementary plus a checkbit in byte+1
467 81726 : if (!((_payload.sensor_scan.startbit == !_payload.sensor_scan.not_startbit) && _payload.sensor_scan.checkbit)) {
468 : Debug(1, "Invalid Payload");
469 0 : _sync_error++;
470 0 : return;
471 : }
472 :
473 81726 : const float angle_sign = (params.orientation == 1) ? -1.0f : 1.0f;
474 81726 : const float angle_deg = wrap_360(_payload.sensor_scan.angle_q6/64.0f * angle_sign + params.yaw_correction);
475 81726 : const float distance_m = (_payload.sensor_scan.distance_q2/4000.0f);
476 : #if RP_DEBUG_LEVEL >= 2
477 : const float quality = _payload.sensor_scan.quality;
478 : Debug(2, " D%02.2f A%03.1f Q%0.2f", distance_m, angle_deg, quality);
479 : #endif
480 81726 : _last_distance_received_ms = AP_HAL::millis();
481 81726 : if (!ignore_reading(angle_deg, distance_m)) {
482 81726 : const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle_deg);
483 :
484 81726 : if (face != _last_face) {
485 : // distance is for a new face, the previous one can be updated now
486 6540 : if (_last_distance_valid) {
487 3818 : frontend.boundary.set_face_attributes(_last_face, _last_angle_deg, _last_distance_m, state.instance);
488 : } else {
489 : // reset distance from last face
490 2722 : frontend.boundary.reset_face(face, state.instance);
491 : }
492 :
493 : // initialize the new face
494 6540 : _last_face = face;
495 6540 : _last_distance_valid = false;
496 : }
497 81726 : if (distance_m > distance_min_m()) {
498 : // update shortest distance
499 13847 : if (!_last_distance_valid || (distance_m < _last_distance_m)) {
500 8594 : _last_distance_m = distance_m;
501 8594 : _last_distance_valid = true;
502 8594 : _last_angle_deg = angle_deg;
503 : }
504 : // update OA database
505 13847 : database_push(_last_angle_deg, _last_distance_m);
506 : }
507 : }
508 : }
509 :
510 0 : void AP_Proximity_RPLidarA2::parse_response_health()
511 : {
512 : // health issue if status is "3" ->HW error
513 0 : if (_payload.sensor_health.status == 3) {
514 : Debug(1, "LIDAR Error");
515 : }
516 : Debug(1, "LIDAR Healthy");
517 0 : }
518 :
519 : #if AP_PROXIMITY_RPLIDAR_EXPRESSSCAN_ENABLED
520 : // verify Dense capsulated (Express) block checksum
521 3387 : bool AP_Proximity_RPLidarA2::verify_cabin_checksum(const uint8_t *buf, size_t len)
522 : {
523 3387 : if (buf == nullptr) {
524 0 : return false;
525 : }
526 :
527 3387 : uint8_t checksum = 0;
528 281121 : for (size_t i = sizeof(_express_header); i < len; i++) {
529 277734 : checksum ^= buf[i];
530 : }
531 :
532 3387 : const uint8_t expected_b0 = uint8_t((EXPRESS_SYNC1 << 4) | (checksum & 0x0F));
533 3387 : const uint8_t expected_b1 = uint8_t((EXPRESS_SYNC2 << 4) | ((checksum >> 4) & 0x0F));
534 :
535 3387 : return (buf[0] == expected_b0) && (buf[1] == expected_b1);
536 : }
537 :
538 :
539 : // parse Dense capsulated Express block
540 3387 : void AP_Proximity_RPLidarA2::parse_response_express(const uint8_t *buf)
541 : {
542 3387 : if (buf == nullptr) {
543 1540 : return;
544 : }
545 :
546 3387 : _last_distance_received_ms = AP_HAL::millis();
547 :
548 : // start_angle_q6: 16-bit, Q6 format at bytes [2..3]
549 3387 : const uint16_t start_angle_q6 = (uint16_t)buf[2] | ((uint16_t)buf[3] << 8);
550 3387 : const float start_angle_deg_raw = start_angle_q6 / 64.0f; // Q6 -> degrees
551 :
552 3387 : const float angle_sign = (params.orientation == 1) ? -1.0f : 1.0f;
553 :
554 : // cabins: 40 cabins * 2-byte distance (mm) starting at buf[4]
555 3387 : const uint8_t *cab = buf + 4;
556 : static constexpr int NUM_CABINS = 40;
557 : // S2 Dense Express default: 32000 samples/s at 10Hz = 0.1125 deg/cabin
558 : static constexpr float DENSE_CABIN_ANGLE_STEP_DEG = 0.1125f;
559 :
560 : // find the shortest valid cabin; rescan only if that cabin is ignored
561 3387 : const float dist_min_m_local = distance_min_m();
562 3387 : const uint8_t *cab_start = cab;
563 3387 : bool have_min = false;
564 3387 : float min_distance_m = 0.0f;
565 3387 : int min_i = 0;
566 :
567 138867 : for (int i = 0; i < NUM_CABINS; i++) {
568 135480 : const uint16_t dist_mm = (uint16_t)cab[0] | ((uint16_t)cab[1] << 8);
569 135480 : cab += 2;
570 135480 : if (dist_mm == 0) {
571 79427 : continue;
572 : }
573 56053 : const float distance_m = dist_mm * 0.001f;
574 56053 : if (distance_m <= dist_min_m_local) {
575 0 : continue;
576 : }
577 56053 : if (!have_min || distance_m < min_distance_m) {
578 24248 : have_min = true;
579 24248 : min_distance_m = distance_m;
580 24248 : min_i = i;
581 : }
582 : }
583 :
584 3387 : if (!have_min) {
585 1540 : return;
586 : }
587 :
588 1847 : const float min_cab_angle_raw = start_angle_deg_raw + min_i * DENSE_CABIN_ANGLE_STEP_DEG;
589 1847 : float min_angle_deg = wrap_360(min_cab_angle_raw * angle_sign + params.yaw_correction);
590 :
591 1847 : if (ignore_reading(min_angle_deg, min_distance_m)) {
592 : // fallback: rescan with per-cabin ignore filter
593 0 : have_min = false;
594 0 : cab = cab_start;
595 :
596 0 : for (int i = 0; i < NUM_CABINS; i++) {
597 0 : const uint16_t dist_mm = (uint16_t)cab[0] | ((uint16_t)cab[1] << 8);
598 0 : cab += 2;
599 0 : if (dist_mm == 0) {
600 0 : continue;
601 : }
602 0 : const float distance_m = dist_mm * 0.001f;
603 0 : if (distance_m <= dist_min_m_local) {
604 0 : continue;
605 : }
606 0 : const float cab_angle_raw = start_angle_deg_raw + i * DENSE_CABIN_ANGLE_STEP_DEG;
607 0 : const float angle_deg = wrap_360(cab_angle_raw * angle_sign + params.yaw_correction);
608 0 : if (ignore_reading(angle_deg, distance_m)) {
609 0 : continue;
610 : }
611 0 : if (!have_min || distance_m < min_distance_m) {
612 0 : have_min = true;
613 0 : min_distance_m = distance_m;
614 0 : min_angle_deg = angle_deg;
615 : }
616 : }
617 :
618 0 : if (!have_min) {
619 0 : return;
620 : }
621 : }
622 :
623 1847 : const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(min_angle_deg);
624 :
625 1847 : if (face != _last_face) {
626 : // distance is for a new face, the previous one can be updated now
627 198 : if (_last_distance_valid) {
628 197 : frontend.boundary.set_face_attributes(_last_face, _last_angle_deg, _last_distance_m, state.instance);
629 : } else {
630 : // reset distance from last face
631 1 : frontend.boundary.reset_face(face, state.instance);
632 : }
633 :
634 : // initialize the new face
635 198 : _last_face = face;
636 198 : _last_distance_valid = false;
637 : }
638 :
639 : // update shortest distance for this face
640 1847 : if (!_last_distance_valid || (min_distance_m < _last_distance_m)) {
641 595 : _last_distance_m = min_distance_m;
642 595 : _last_distance_valid = true;
643 595 : _last_angle_deg = min_angle_deg;
644 : }
645 : // update OA database
646 1847 : database_push(_last_angle_deg, _last_distance_m);
647 : }
648 :
649 13548 : bool AP_Proximity_RPLidarA2::express_time_exceeded(uint32_t start_us) const
650 : {
651 : static constexpr uint32_t MAX_US = 1000;
652 13548 : return (AP_HAL::micros() - start_us) > MAX_US;
653 : }
654 :
655 0 : void AP_Proximity_RPLidarA2::ensure_express_stream_space(uint16_t need)
656 : {
657 0 : if (_express_stream_len + need <= EXPRESS_STREAM_BUFFER_SIZE) {
658 0 : return;
659 : }
660 :
661 0 : uint16_t keep = EXPRESS_STREAM_BUFFER_SIZE / 2;
662 0 : keep = MIN(keep, _express_stream_len);
663 :
664 0 : if (keep == 0) {
665 0 : _express_stream_len = 0;
666 0 : _sync_error = 0;
667 : Debug(1, "EXPRESS stream overflow, dropping all data");
668 0 : return;
669 : }
670 :
671 0 : const uint16_t src = _express_stream_len - keep;
672 0 : if (src != 0) {
673 0 : memmove(_express_stream, _express_stream + src, keep);
674 : }
675 0 : _express_stream_len = keep;
676 0 : _sync_error = 0;
677 : Debug(1, "EXPRESS stream overflow, dropping old data");
678 : }
679 :
680 8130 : void AP_Proximity_RPLidarA2::handle_express_data()
681 : {
682 8130 : const uint32_t start_us = AP_HAL::micros();
683 :
684 8130 : uint16_t bytes_read_total = 0;
685 :
686 11517 : while (_uart->available() && bytes_read_total < EXPRESS_MAX_BYTES_CONSUME) {
687 3387 : if (express_time_exceeded(start_us)) {
688 0 : break;
689 : }
690 :
691 3387 : const uint16_t remaining = uint16_t(EXPRESS_MAX_BYTES_CONSUME - bytes_read_total);
692 3387 : uint16_t space = EXPRESS_STREAM_BUFFER_SIZE - _express_stream_len;
693 3387 : if (space == 0) {
694 0 : ensure_express_stream_space(1);
695 0 : space = EXPRESS_STREAM_BUFFER_SIZE - _express_stream_len;
696 0 : if (space == 0) {
697 0 : break;
698 : }
699 : }
700 3387 : const uint16_t to_read = MIN(space, remaining);
701 :
702 3387 : if (to_read == 0) {
703 0 : break;
704 : }
705 :
706 3387 : const uint16_t n = _uart->read(_express_stream + _express_stream_len, to_read);
707 3387 : if (n == 0) {
708 0 : break;
709 : }
710 :
711 3387 : _express_stream_len += n;
712 3387 : bytes_read_total += n;
713 : }
714 :
715 : // scan the stream buffer for valid headers and process full blocks
716 8130 : uint16_t idx = 0;
717 :
718 11517 : while (_express_stream_len >= idx + 2) {
719 3387 : if (express_time_exceeded(start_us)) {
720 0 : break;
721 : }
722 :
723 : // search for a header candidate (upper nibbles 0xA / 0x5)
724 3387 : while (idx + 1 < _express_stream_len) {
725 3387 : if (express_time_exceeded(start_us)) {
726 0 : break;
727 : }
728 3387 : const uint8_t b0 = _express_stream[idx];
729 3387 : const uint8_t b1 = _express_stream[idx + 1];
730 3387 : if ((b0 >> 4) == EXPRESS_SYNC1 && (b1 >> 4) == EXPRESS_SYNC2) {
731 : // header candidate found
732 3387 : break;
733 : }
734 0 : idx++;
735 : }
736 :
737 3387 : if (express_time_exceeded(start_us)) {
738 0 : break;
739 : }
740 :
741 3387 : if (idx + 1 >= _express_stream_len) {
742 : // no header candidate found, drop all accumulated data
743 0 : if (_express_stream_len > 0) {
744 : Debug(1, "EXPRESS: no header candidate, dropping %u bytes", unsigned(_express_stream_len));
745 0 : _express_stream[0] = _express_stream[_express_stream_len - 1];
746 0 : _express_stream_len = 1;
747 : } else {
748 0 : _express_stream_len = 0;
749 : }
750 0 : _sync_error = 0;
751 0 : return;
752 : }
753 :
754 : // idx now points to a header candidate
755 3387 : if (_express_stream_len - idx < EXPRESS_BLOCK_SIZE) {
756 : // not enough bytes for a full block
757 : // move remaining bytes to the front and wait for more data
758 0 : if (idx > 0) {
759 0 : memmove(_express_stream,
760 0 : _express_stream + idx,
761 0 : _express_stream_len - idx);
762 0 : _express_stream_len -= idx;
763 : }
764 0 : return;
765 : }
766 :
767 : // pointer to the candidate block
768 3387 : uint8_t *blk = _express_stream + idx;
769 :
770 3387 : if (!verify_cabin_checksum(blk, EXPRESS_BLOCK_SIZE)) {
771 : // upper nibbles match but checksum is invalid
772 : // advance by one byte and try to resynchronise
773 : Debug(1, "EXPRESS/DENSE checksum error");
774 0 : _sync_error++;
775 :
776 0 : if (_sync_error > 10) {
777 0 : reset_rplidar();
778 0 : _express_stream_len = 0;
779 0 : return;
780 : }
781 :
782 0 : idx++;
783 0 : continue;
784 : }
785 :
786 : // valid block found
787 3387 : _sync_error = 0;
788 3387 : parse_response_express(blk);
789 :
790 : // consume this block and look for the next one
791 3387 : idx += EXPRESS_BLOCK_SIZE;
792 : }
793 :
794 : // compact any remaining unprocessed bytes to the front of the buffer
795 8130 : if (idx > 0 && idx <= _express_stream_len) {
796 3387 : const uint16_t remaining = _express_stream_len - idx;
797 3387 : if (remaining > 0) {
798 0 : memmove(_express_stream,
799 0 : _express_stream + idx,
800 : remaining);
801 : }
802 3387 : _express_stream_len = remaining;
803 : }
804 : }
805 : #endif // AP_PROXIMITY_RPLIDAR_EXPRESSSCAN_ENABLED
806 :
807 : #endif // AP_PROXIMITY_RPLIDARA2_ENABLED
|
