Hello everybody!
I just bought the LD20 LiDAR but cannot make it work with my Arduino Uno R3. It works, actually, I’m getting data, however, the numbers are strange, the distance is always too far, even when there is an object in front of it.
I trying looking at the SDK, but didn’t make it work ">
Could you help me, please?
Here are some codes that I’m using. Example 1:
#include <SoftwareSerial.h>
#include <Arduino.h>
#define POINTS_PER_PACK 12
#define HEADER 0x54
#define LIDAR_BAUD 230400
#define POINT_BUFF_SZ 360
typedef struct __attribute__((packed)) {
uint16_t distance;
uint8_t intensity;
} LidarPoint;
typedef struct __attribute__((packed)) {
uint8_t header;
uint8_t ver_len;
uint16_t speed;
uint16_t start_angle;
LidarPoint point[POINTS_PER_PACK];
uint16_t end_angle;
uint16_t timestamp;
uint8_t crc8;
} LiDARFrame;
static const uint8_t CrcTable[256] = {
0x00, 0x4d, 0x9a, 0xd7, 0x79, 0x34, 0xe3,
0xae, 0xf2, 0xbf, 0x68, 0x25, 0x8b, 0xc6, 0x11, 0x5c, 0xa9, 0xe4, 0x33,
0x7e, 0xd0, 0x9d, 0x4a, 0x07, 0x5b, 0x16, 0xc1, 0x8c, 0x22, 0x6f, 0xb8,
0xf5, 0x1f, 0x52, 0x85, 0xc8, 0x66, 0x2b, 0xfc, 0xb1, 0xed, 0xa0, 0x77,
0x3a, 0x94, 0xd9, 0x0e, 0x43, 0xb6, 0xfb, 0x2c, 0x61, 0xcf, 0x82, 0x55,
0x18, 0x44, 0x09, 0xde, 0x93, 0x3d, 0x70, 0xa7, 0xea, 0x3e, 0x73, 0xa4,
0xe9, 0x47, 0x0a, 0xdd, 0x90, 0xcc, 0x81, 0x56, 0x1b, 0xb5, 0xf8, 0x2f,
0x62, 0x97, 0xda, 0x0d, 0x40, 0xee, 0xa3, 0x74, 0x39, 0x65, 0x28, 0xff,
0xb2, 0x1c, 0x51, 0x86, 0xcb, 0x21, 0x6c, 0xbb, 0xf6, 0x58, 0x15, 0xc2,
0x5f, 0xf1, 0xbc, 0x6b, 0x26, 0x7a, 0x37, 0xe0, 0xad, 0x03, 0x4e, 0x99,
0xd4, 0x7c, 0x31, 0xe6, 0xab, 0x05, 0x48, 0x9f, 0xd2, 0x8e, 0xc3, 0x14,
0x59, 0xf7, 0xba, 0x6d, 0x20, 0xd5, 0x98, 0x4f, 0x02, 0xac, 0xe1, 0x36,
0x7b, 0x27, 0x6a, 0xbd, 0xf0, 0x5e, 0x13, 0xc4, 0x89, 0x63, 0x2e, 0xf9,
0xb4, 0x1a, 0x57, 0x80, 0xcd, 0x91, 0xdc, 0x0b, 0x46, 0xe8, 0xa5, 0x72,
0x3f, 0xca, 0x87, 0x50, 0x1d, 0xb3, 0xfe, 0x29, 0x64, 0x38, 0x75, 0xa2,
0xef, 0x41, 0x0c, 0xdb, 0x96, 0x42, 0x0f, 0xd8, 0x95, 0x3b, 0x76, 0xa1,
0xec, 0xb0, 0xfd, 0x2a, 0x67, 0xc9, 0x84, 0x53, 0x1e, 0xeb, 0xa6, 0x71,
0x3c, 0x92, 0xdf, 0x08, 0x45, 0x19, 0x54, 0x83, 0xce, 0x60, 0x2d, 0xfa,
0xb7, 0x5d, 0x10, 0xc7, 0x8a, 0x24, 0x69, 0xbe, 0xf3, 0xaf, 0xe2, 0x35,
0x78, 0xd6, 0x9b, 0x4c, 0x01, 0xf4, 0xb9, 0x6e, 0x23, 0x8d, 0xc0, 0x17,
0x5a, 0x06, 0x4b, 0x9c, 0xd1, 0x7f, 0x32, 0xe5, 0xa8
};
uint8_t CalCRC8(uint8_t *p, uint8_t len) {
uint8_t crc = 0;
for (int i = 0; i < len; i++) {
crc = CrcTable[(crc ^ *p++) & 0xff];
}
return crc;
}
// Change the RX and TX pins as per your connections.
SoftwareSerial mySerial(10, 11); // RX, TX
LiDARFrame lidarFrame;
void setup() {
Serial.begin(9600);
// set the data rate for the SoftwareSerial port
mySerial.begin(LIDAR_BAUD);
}
void loop() {
if (mySerial.available()) {
if (mySerial.read() == HEADER) {
mySerial.readBytes((char*)&lidarFrame + 1, sizeof(LiDARFrame) - 1);
if (CalCRC8((uint8_t*)&lidarFrame, sizeof(LiDARFrame) - 1) == lidarFrame.crc8) {
double step = (lidarFrame.end_angle - lidarFrame.start_angle) / (POINTS_PER_PACK - 1);
for (int i = 0; i < POINTS_PER_PACK; i++) {
double angle = (lidarFrame.start_angle + step * i);
angle = fmod(angle + 360, 360);
//angle = 360 - angle; // Convert to counterclockwise direction
angle += 90;
angle = fmod(angle + 360, 360);
int distance = lidarFrame.point[i].distance;
uint8_t intensity = lidarFrame.point[i].intensity;
if (intensity < 10) continue;
if (distance > 0 and distance <= 200) {
Serial.print(angle);
Serial.print(F(" degrees, distance: "));
Serial.print(distance);
Serial.print(F(", intensity: "));
Serial.println(intensity);
}
}
} else {
//Serial.println("CRC check failed!");
}
}
}
}
Example 2 (with more debugging):
#include <SoftwareSerial.h>
#include <Arduino.h>
#define LIDAR_BAUD 230400
SoftwareSerial mySerial(10, 11); // RX, TX
#define POINTS_PER_PACK 12
#define HEADER 0x54
#define VERLEN 0x2C
/*
* Initial Identifier: 1 Byte, fixed value of 0x54
* VerLen: 1 Byte, fixed value of 0x2C
* Radar Speed: 2 Bytes, unit is degrees per second
* Starting Angle: 2 Bytes, unit is 0.01 degrees
* Data: Variable, each measurement data is 3 Bytes
* End Angle: 2 Bytes, unit is 0.01 degrees
* Timestamp: 2 Bytes, unit is milliseconds, counts from 30000
* CRC Check: Variable, checksum of all previous data
*/
#define PACKET_SIZE (1 + 1 + 2 + 2 + (POINTS_PER_PACK * 3) + 2 + 2 + 1)
byte packet[PACKET_SIZE];
int idx = 0;
void setup() {
Serial.begin(115200);
// set the data rate for the SoftwareSerial port
mySerial.begin(LIDAR_BAUD);
}
void loop() {
if (mySerial.available()) {
byte incomingByte = mySerial.read();
if (idx == 0 && incomingByte != HEADER) {
return; // Not a header, skip
}
packet[idx] = incomingByte;
idx++;
if (idx >= PACKET_SIZE) {
// Full packet captured
idx = 0; // Reset index
processPacket();
}
}
}
void HexDebug(int hex1, int hex2, int result) {
Serial.print("HEX: ");
Serial.print(hex1, HEX);
Serial.print(" ");
Serial.print(hex2, HEX);
Serial.print(" - little-endian: ");
Serial.print(hex2, HEX);
Serial.print(hex1, HEX);
Serial.print("H - Converted: ");
Serial.println(result);
}
void processPacket() {
if (packet[0] != HEADER) {
Serial.println("Wrong header");
return;
}
if (packet[1] != VERLEN) {
//Serial.println("Wrong VERLEN");
return;
}
//Serial.print("Full Packet: ");
//for (int j = 0; j < PACKET_SIZE; j++) {
//Serial.print(packet[j], HEX);
//Serial.print(" ");
//}
//Serial.println();
//Serial.print("Header: ");
//Serial.println(packet[0], HEX);
//Serial.print("VerLen: ");
//Serial.println(packet[1], HEX);
// Radar speed
uint16_t radarSpeed = packet[3] << 8 | packet[2];
//Serial.print("Radar speed: ");
//HexDebug(packet[2], packet[3], radarSpeed);
// Start angle
uint16_t startAngle = packet[5] << 8 | packet[4];
//Serial.print("Start angle: ");
//HexDebug(packet[4], packet[5], startAngle);
// End angle
uint16_t endAngle = packet[PACKET_SIZE - 5] << 8 | packet[PACKET_SIZE - 4];
//Serial.print("End angle: ");
//HexDebug(packet[PACKET_SIZE - 4], packet[PACKET_SIZE - 5], endAngle);
// Timestamp
uint16_t timestamp = packet[PACKET_SIZE - 2] << 8 | packet[PACKET_SIZE - 3];
//Serial.print("Timestamp: ");
//HexDebug(packet[PACKET_SIZE - 3], packet[PACKET_SIZE - 2], timestamp);
// CRC
byte crc = packet[PACKET_SIZE - 1];
//Serial.print("CRC: ");
//Serial.println(crc, HEX);
// Process lidar points
for (int i = 0; i < POINTS_PER_PACK; i++) {
uint16_t distance = packet[7 + i * 3] << 8 | packet[6 + i * 3];
uint8_t intensity = packet[8 + i * 3];
// Print distance HEX value
//Serial.print("- Packet ");
//Serial.print(i);
//Serial.print(": ");
//HexDebug(packet[6 + i * 3], packet[7 + i * 3], distance);
if (distance == 0) {
// Invalid distance
continue;
}
if (intensity < 200) {
// Invalid intensity
continue;
}
// Calculate angle based on start angle, end angle, and index
float step = float(endAngle - startAngle) / (POINTS_PER_PACK - 1);
float angle = startAngle + step * i;
angle *= 0.01;
// Max angle of 360 degrees
angle = fmod(angle, 360);
//Serial.print("Point ");
//Serial.print(i);
Serial.print(": Distance ");
Serial.print(distance);
// Print distance in cm
//Serial.print(distance / 10);
Serial.print(" mm, Intensity ");
Serial.print(intensity);
Serial.print(", Angle ");
Serial.println(angle, 2);
}
}