MysteryLidar: Difference between revisions
(9 intermediate revisions by the same user not shown) | |||
Line 18: | Line 18: | ||
* on the PCB: data matrix FW0251022426001437 | * on the PCB: data matrix FW0251022426001437 | ||
* next to the power/data connector: 2d bar code 20617158 | * next to the power/data connector: 2d bar code 20617158 | ||
Connecting a serial port: | |||
* green: 5V | |||
* yellow: TX | |||
* black: ground | |||
* red: RX | |||
== Protocol == | == Protocol == | ||
Line 45: | Line 51: | ||
The angle is encoded in steps of 1/64 degree, with a 640 offset. | The angle is encoded in steps of 1/64 degree, with a 640 offset. | ||
Example packet: | |||
|<-- header -->| |<------------------------------------------------------------------data--------------------------------------------------------------------->| |<footer->| | |||
55 aa 23 10 ec 59 ac f7 7f 40 cd 7f 40 cc 7f 40 cf 7f 40 c6 7f 40 bf 7f 40 b4 80 40 b9 80 40 ba 80 40 bc 80 40 ba 80 40 b5 80 40 d0 81 40 ca 81 40 c2 82 40 c2 82 40 c8 0e a0 cd 2e | |||
=== Throughput === | === Throughput === | ||
Line 51: | Line 61: | ||
* each packet contains 16 measurements | * each packet contains 16 measurements | ||
* each packet contains data for 10 degrees | * each packet contains data for 10 degrees | ||
* there are about | * there are about 36 packets per rotation | ||
* there are about 6 rotations per second | * there are about 6 rotations per second | ||
* => 6 rotations/s * 36 packets * 60 bytes = 12960 bytes/s | * => 6 rotations/s * 36 packets * 60 bytes = 12960 bytes/s | ||
* => 6 rotations/s * 36 packets * 16 samples/packet = 3456 samples/s | * => 6 rotations/s * 36 packets * 16 samples/packet = 3456 samples/s | ||
=== header === | === header === | ||
Line 64: | Line 70: | ||
* 0x55 0xAA fixed header | * 0x55 0xAA fixed header | ||
* 0x23 = fixed number = packet type? | * 0x23 = fixed number = packet type? | ||
* 0x10 = number of samples | * 0x10 = number of samples in this packet | ||
* 0x59EC = 23020, angular resolution, in units of 1/64th step of a degree, nominally 360*64 = 23040 | * 0x59EC = 23020, angular resolution, in units of 1/64th step of a degree, nominally 360*64 = 23040 | ||
* 0xF7AC = start angle => 350 degrees | * 0xF7AC = start angle => 350 degrees | ||
Start and end angles are encoded in 1/64th of a degree. | |||
Encoded number = 640 + 64*angle. | |||
* 0e a0 = 40974 => 0 degrees | |||
=== ranging data === | === ranging data === | ||
Line 73: | Line 83: | ||
* last is intensity, e.g. 0xCD | * last is intensity, e.g. 0xCD | ||
=== | === footer === | ||
We see the following bytes: | |||
* 0e a0 => 0xA00E = final angle | |||
* | * cd => checksum | ||
* 2e => unknown, is probably not a checksum, I've seen packets that are identical except for this byte | |||
=== check === | === check === | ||
Line 85: | Line 96: | ||
== Software == | == Software == | ||
See https://github.com/bertrik/MysteryLidar | See https://github.com/bertrik/MysteryLidar | ||
This software allows you to see the raw data coming from the lidar. | |||
It also shows what the lidar sees as a 2d-point cloud in a graphical display. |
Latest revision as of 16:08, 10 August 2024
Project MysteryLidar | |
---|---|
Aliexpress Mystery Lidar | |
Status | In progress |
Contact | bertrik |
Last Update | 2024-08-10 |
Introduction
I bought an inexpensive lidar from AliExpress, no guarantees, no datasheets. This page is about understanding it and reverse engineering the protocol
Hardware
On Aliexpress: https://nl.aliexpress.com/item/1005007119470172.html and https://nl.aliexpress.com/item/1005007204489515.html
Has stickers with the following codes:
- on the side of the plastic casing: data matrix HHCT3B6X9K00863
- on the PCB: data matrix FW0251022426001437
- next to the power/data connector: 2d bar code 20617158
Connecting a serial port:
- green: 5V
- yellow: TX
- black: ground
- red: RX
Protocol
Serial bit rate is 230400 bps.
Looks a bit similar (but not identical) to other LIDAR protocols:
- https://github.com/YDLIDAR/YDLidar-SDK/blob/master/doc/YDLidar-SDK-Communication-Protocol.md
- https://www.waveshare.com/wiki/D200_LiDAR_Kit
- https://gibbard.me/lidar/
From the Aliexpress page
Baud: 230400; Packet_length=60Bytes (in every ~4ms period)
- B0= 0x55
- B1=0xAA
- B2=0x23
- B3=0x10
- B4+B5=current resolution LSB+MSB
- B6+B7= Start Angle of this packet LSB+MSB
- 16x:
- B8:B9= Distance LSB+MSB
- B10= Quality (1 byte only) . . .
- B56+B57= Final Angle of this packet (LSB+MSB)
- B58+B59= CRC
The angle is encoded in steps of 1/64 degree, with a 640 offset.
Example packet:
|<-- header -->| |<------------------------------------------------------------------data--------------------------------------------------------------------->| |<footer->| 55 aa 23 10 ec 59 ac f7 7f 40 cd 7f 40 cc 7f 40 cf 7f 40 c6 7f 40 bf 7f 40 b4 80 40 b9 80 40 ba 80 40 bc 80 40 ba 80 40 b5 80 40 d0 81 40 ca 81 40 c2 82 40 c2 82 40 c8 0e a0 cd 2e
Throughput
Some numbers:
- each packet is 60 bytes
- each packet contains 16 measurements
- each packet contains data for 10 degrees
- there are about 36 packets per rotation
- there are about 6 rotations per second
- => 6 rotations/s * 36 packets * 60 bytes = 12960 bytes/s
- => 6 rotations/s * 36 packets * 16 samples/packet = 3456 samples/s
header
We see the following bytes:
- 0x55 0xAA fixed header
- 0x23 = fixed number = packet type?
- 0x10 = number of samples in this packet
- 0x59EC = 23020, angular resolution, in units of 1/64th step of a degree, nominally 360*64 = 23040
- 0xF7AC = start angle => 350 degrees
Start and end angles are encoded in 1/64th of a degree. Encoded number = 640 + 64*angle.
- 0e a0 = 40974 => 0 degrees
ranging data
Each sample is encoded in 3 bytes:
- first 2 are distance, e.g. 0x0407F, highest bits are not part of the range
- last is intensity, e.g. 0xCD
We see the following bytes:
- 0e a0 => 0xA00E = final angle
- cd => checksum
- 2e => unknown, is probably not a checksum, I've seen packets that are identical except for this byte
check
I have not been able to figure out what the checksum is, a simple checksum, or a CRC, or even it is an actual checksum at all.
I noticed that bit 15 is always 0, this suggests some kind of 15-bit CRC, like the CANBUS CRC.
Software
See https://github.com/bertrik/MysteryLidar
This software allows you to see the raw data coming from the lidar. It also shows what the lidar sees as a 2d-point cloud in a graphical display.