Đọc lidar LD14 bằng ESP32 và xem ảnh quét ngay trên trình duyệt

Hướng dẫn này đọc dữ liệu lidar LDROBOT LD14 bằng ESP32 chạy ESP-IDF (không dùng Arduino) và hiển thị ảnh quét trên trình duyệt qua WiFi. LD14 tự phát dữ liệu ra UART ngay khi cấp điện, không cần lệnh khởi tạo hay thư viện đi kèm. Toàn bộ chương trình nằm trong một file main.c khoảng 360 dòng; phần cuối bài có hướng dẫn dùng trực tiếp với ROS2.

Code mẫu: github.com/dungtsbb/ld14_web

1. Chuẩn bị

Món	Ghi chú
ESP32 DevKit (WROOM)	Xem lưu ý về WROVER ở phần nối dây
Lidar LDROBOT LD14	LD06 / LD19 dùng được cùng code, chỉ khác baud
Nguồn 5V ≥ 500mA	Lidar có động cơ, cần nguồn đủ dòng lúc khởi động
ESP-IDF v5.x hoặc v6.x	Bài này build trên v6.0.2

2. Nối dây

LD14 ra 4 dây, cần nối 3:

Dây lidar	Nối vào ESP32	Ghi chú
TX (dữ liệu)	GPIO 16 (UART2 RX)	Lidar chỉ phát, không nhận lệnh
5V	5V / VIN	Qua nguồn riêng nếu chạy pin
GND	GND	Phải chung mass với ESP32
PWM (điều khiển tốc độ)	bỏ trống	Không nối thì lidar chạy tốc độ mặc định

Mặt dưới LD14 với 4 dây: xanh lá là 5V, vàng là TX, đen là GND, đỏ là PWM không dùng

Trong ảnh: dây xanh lá là 5V, vàng là TX, đen là GND, dây đỏ (PWM) bỏ trống. Màu dây có thể khác giữa các lô hàng — đối chiếu với nhãn in trên board (TX, PWM/RX, 5V, GND).

Ba điểm cần lưu ý:

Mức logic của LD14 là 3.3V — nối thẳng vào GPIO, không cần mạch chia áp.
Chỉ nối TX của lidar vào RX của ESP32. Đây là giao tiếp một chiều; nối nhầm TX↔TX thì không có dữ liệu và cũng không có thông báo lỗi.
Module WROVER có GPIO 16/17 dành cho PSRAM — dùng cặp chân khác (ví dụ GPIO 4/5) và sửa LIDAR_RX_PIN trong code.

3. Giao thức: 47 byte một gói

LD14 dùng chung giao thức DTOF với LD06/LD19. Mỗi gói dài đúng 47 byte và chứa 12 điểm đo:

0x54 0x2C | speed(2) | góc đầu(2) | 12×[khoảng cách(2) + cường độ(1)] | góc cuối(2) | timestamp(2) | CRC8(1)

0x54 là byte đồng bộ, 0x2C là “phiên bản + độ dài”, cố định với dòng này (0x2C tương ứng 12 điểm mỗi gói).
Góc tính theo đơn vị 0.01 độ — 9000 nghĩa là 90.00°.
Khoảng cách tính theo mm. Giá trị 0 nghĩa là không có vật phản xạ ở hướng này (quá xa, mặt hấp thụ, hoặc kính).
Cường độ 0–255: độ mạnh của tín hiệu phản hồi, dùng để lọc điểm nhiễu.
speed là độ/giây; chia 6 ra vòng/phút.
Một gói chứa 12 điểm trong một cung khoảng 12°; góc của từng điểm nội suy đều giữa góc đầu và góc cuối.

Mọi số nhiều byte đều little-endian, trùng với ESP32, nên chỉ cần khai báo struct và memcpy:

typedef struct __attribute__((packed)) {
    uint16_t distance;    /* mm, 0 = không có vật phản xạ */
    uint8_t  intensity;   /* 0..255 */
} point_raw_t;

typedef struct __attribute__((packed)) {
    uint8_t     header;        /* 0x54 */
    uint8_t     ver_len;       /* 0x2C */
    uint16_t    speed;         /* độ/giây */
    uint16_t    start_angle;   /* 0.01° */
    point_raw_t points[12];
    uint16_t    end_angle;     /* 0.01° */
    uint16_t    timestamp;
    uint8_t     crc8;
} lidar_packet_t;

/* Nếu trình biên dịch chèn byte đệm thì mọi phép đọc phía sau đều lệch.
 * Chốt cứng kích thước ngay lúc biên dịch. */
_Static_assert(sizeof(lidar_packet_t) == 47, "gói phải đúng 47 byte");

__attribute__((packed)) là bắt buộc: thiếu nó, trình biên dịch chèn byte đệm để uint16_t nằm ở địa chỉ chẵn, struct phình lên 48–52 byte và mọi trường sau header đều đọc sai.

Kiểm CRC8

Byte cuối là CRC8 với đa thức 0x4D, MSB-first, khởi tạo 0 — không trùng với các biến thể CRC8 thông dụng, cần dựng đúng bảng tra theo đa thức này.

Lý do phải kiểm CRC: byte 0x54 có thể xuất hiện giữa một giá trị khoảng cách. Nếu chỉ dò header mà không kiểm CRC, thỉnh thoảng parser sẽ nhận một gói giả và trả về điểm đo sai.

static uint8_t s_crc_table[256];

static void crc_table_init(void)
{
    for (int i = 0; i < 256; i++) {
        uint8_t crc = (uint8_t)i;
        for (int b = 0; b < 8; b++) {
            crc = (crc & 0x80) ? (uint8_t)((crc << 1) ^ 0x4D) : (uint8_t)(crc << 1);
        }
        s_crc_table[i] = crc;
    }
}

Bảng tra 256 phần tử dựng một lần lúc khởi động, đổi 8 phép dịch bit mỗi byte thành một lần tra bảng. LD14 phát khoảng 192 gói mỗi giây (~9000 byte/giây cần kiểm).

4. Dò gói trong dòng byte

UART trả về từng khúc byte tuỳ ý, có thể cắt ngang giữa gói. Vòng lặp dò xử lý như sau:

static size_t parse_buffer(const uint8_t *buf, size_t len)
{
    size_t i = 0;
    while (i + PKT_SIZE <= len) {
        if (buf[i] != PKT_HEADER || buf[i + 1] != PKT_VERLEN) {
            i++;
            continue;
        }
        if (crc8(buf + i, PKT_SIZE - 1) != buf[i + PKT_SIZE - 1]) {
            s_err_count++;
            i++;         /* CRC sai: có thể là header giả, trượt 1 byte */
            continue;
        }
        lidar_packet_t pkt;
        memcpy(&pkt, buf + i, PKT_SIZE);
        handle_packet(&pkt);
        i += PKT_SIZE;
    }
    return i;   /* số byte đã tiêu thụ; phần đuôi giữ lại cho lần sau */
}

Hàm trả về số byte đã tiêu thụ; phần đuôi chưa đủ một gói được memmove về đầu buffer chờ lần đọc tiếp. Khi CRC sai, trượt 1 byte chứ không phải 47 — rất có thể vừa gặp một header giả và gói thật nằm ngay phía sau.

5. Gom 12 điểm/gói thành một vòng 360°

Mỗi gói là một cung nhỏ. Để có ảnh quét đầy đủ, chia vòng tròn thành 360 ô 1° và đổ điểm vào ô tương ứng:

static void handle_packet(const lidar_packet_t *p)
{
    float start = p->start_angle * 0.01f;
    float end   = p->end_angle * 0.01f;
    float span  = end - start;
    if (span < 0.0f) span += 360.0f;      /* gói vắt qua mốc 0° */
    float step = span / 11;               /* 12 điểm = 11 khoảng */

    /* Lidar không có cờ "hết vòng". Góc đầu gói nhỏ hơn gói trước nghĩa là
     * kim đã quay qua mốc 0°, tức vừa xong một vòng. */
    if (s_last_start_deg >= 0.0f && start < s_last_start_deg) {
        finish_scan();
    }
    s_last_start_deg = start;

    for (int i = 0; i < 12; i++) {
        uint16_t d = p->points[i].distance;
        if (d == 0) continue;
        float ang = start + step * i;
        while (ang >= 360.0f) ang -= 360.0f;
        int bin = (int)(ang + 0.5f) % 360;
        /* Nhiều điểm rơi cùng một ô: giữ điểm có tín hiệu mạnh hơn */
        if (s_building.dist_mm[bin] == 0 ||
            p->points[i].intensity > s_building.intensity[bin]) {
            s_building.dist_mm[bin] = d;
            s_building.intensity[bin] = p->points[i].intensity;
        }
    }
}

Ba điểm cần lưu ý khi triển khai:

Phát hiện hết vòng. Lidar không gửi cờ báo xong một vòng. Dấu hiệu duy nhất: góc đầu gói nhỏ hơn góc đầu của gói trước — kim vừa vượt mốc 0°.

Buffer kép. Task lidar ghi vào s_building, web server đọc s_complete. Dùng chung một buffer thì trang web sẽ đọc phải ảnh nửa cũ nửa mới. Lúc chốt vòng, memcpy sang buffer kia trong taskENTER_CRITICAL — khoảng 1KB, vài micro giây, đủ ngắn cho critical section.

Ưu tiên task. Task đọc UART cần ưu tiên cao (bài này dùng 13, mức thường là 5). Buffer UART 2KB chỉ chứa khoảng 0.2 giây dữ liệu; task bị chen là mất gói, biểu hiện là ảnh quét khuyết từng miếng.

6. Xuất ra web

esp_http_server của ESP-IDF có sẵn, cần hai endpoint:

GET / → trang HTML (nhúng vào firmware bằng EMBED_FILES)
GET /scan → {"rpm":..,"seq":..,"err":..,"d":[360 số mm]}

Nhúng file HTML vào firmware bằng một dòng trong main/CMakeLists.txt:

idf_component_register(SRCS "main.c"
                       INCLUDE_DIRS "."
                       EMBED_FILES "index.html")

rồi trong C:

extern const uint8_t index_html_start[] asm("_binary_index_html_start");
extern const uint8_t index_html_end[]   asm("_binary_index_html_end");

JSON cho 360 số dài khoảng 2KB. Thay vì nối cả chuỗi trong RAM rồi gửi một lần, dùng httpd_resp_send_chunk gửi từng khúc 256 byte — chỉ tốn một buffer nhỏ trên stack:

char part[256];
int pos = 0;
for (int i = 0; i < SCAN_BINS; i++) {
    pos += snprintf(part + pos, sizeof(part) - pos, "%s%u",
                    i ? "," : "", snap.dist_mm[i]);
    if (pos > (int)sizeof(part) - 8) {
        httpd_resp_send_chunk(req, part, pos);
        pos = 0;
    }
}

JSON phù hợp khi cần đọc hiểu dữ liệu. Nếu cần băng thông thấp hơn, gửi thẳng 720 byte nhị phân (Int16Array bên JS) — nhẹ hơn khoảng ba lần.

7. Vẽ radar bằng canvas

Trang HTML hơn 80 dòng. Phần lõi là đổi toạ độ cực sang toạ độ canvas:

for (let a = 0; a < d.length; a++) {
  const mm = d[a];
  if (mm === 0) continue;             // 0 = không có vật phản xạ
  const m = mm / 1000;
  if (m > rangeM) continue;
  const rad = a * Math.PI / 180;
  const x = R + R * m / rangeM * Math.sin(rad);
  const y = R - R * m / rangeM * Math.cos(rad);   // trục y canvas hướng xuống
  ctx.fillRect(x - 1.5, y - 1.5, 3, 3);
}

Chú ý dấu trừ ở y: trục y của canvas hướng xuống, không đảo dấu thì ảnh quét bị lộn ngược.

Vòng lặp cập nhật dùng setTimeout sau khi xử lý xong, không dùng setInterval — gặp mạng chậm, setInterval làm các yêu cầu dồn lên nhau:

async function poll() {
  try { draw((await (await fetch('/scan')).json()).d); } catch (e) {}
  setTimeout(poll, 150);
}

8. Chạy thử

git clone https://github.com/dungtsbb/ld14_web.git
cd ld14_web
# Sửa WIFI_SSID và WIFI_PASS ở đầu main/main.c
idf.py set-target esp32
idf.py build flash monitor

Trên màn hình monitor:

I (2870) ld14: Mở trình duyệt tại: http://192.168.1.42
I (4000) ld14: vòng quét=28  gói lỗi CRC=0  rpm=360

Mở địa chỉ đó trên thiết bị cùng mạng WiFi. Theo dõi chỉ số gói lỗi CRC — tăng liên tục nghĩa là vấn đề nằm ở dây hoặc nguồn, không phải code.

9. Gỡ lỗi những ca hay gặp

Hiện tượng	Nguyên nhân thường gặp
`vòng quét=0`, `lỗi CRC=0`	Không có byte nào vào: sai chân, hoặc nối TX↔TX. Kiểm tra bằng cách nối tạm TX lidar vào RX của USB-serial
`lỗi CRC` tăng vọt, `vòng quét` vẫn nhích	Sai baud. LD14 = 115200, LD06/LD19 = 230400. Đổi `LIDAR_BAUD`
Lidar quay vài giây rồi dừng	Thiếu nguồn: động cơ kéo dòng cao lúc khởi động
Ảnh quét khuyết từng miếng	Task đọc UART bị chen. Tăng ưu tiên task hoặc tăng buffer UART
Ảnh lộn ngược / lệch 90°	Phép đổi toạ độ phía JS. Đổi dấu hoặc cộng offset vào góc
Rpm khoảng 60 thay vì ~360	`speed` là độ/giây, phải chia 6 ra vòng/phút

10. Hướng mở rộng

Đổi JSON sang nhị phân + WebSocket để đạt đủ 10 khung/giây.
Lọc theo cường độ: bỏ các điểm intensity thấp để giảm nhiễu.
Ghép với odometry (encoder bánh xe) để dựng bản đồ — bước đầu của SLAM. ESP32 đủ sức chạy occupancy grid 10×10m ở độ phân giải 5cm cùng scan matching tương quan ngay trên chip.

Toàn bộ code trong bài build trên ESP-IDF v6.0.2, firmware khoảng 835KB.

11. Dùng với ROS2

Khi lidar lắp lên robot chạy ROS2 (Orange Pi, Raspberry Pi, PC…), kết nối trực tiếp qua mạch USB–UART (CP2102, CH340…) và dùng driver chính thức của LDROBOT — ldrobotSensorTeam/ldlidar_ros2 (hỗ trợ LD06 / LD14 / LD14P / LD19). Nối dây như phần 2: TX lidar → RX của mạch USB–UART, cộng 5V và GND.

mkdir -p ~/ldlidar_ros2_ws/src && cd ~/ldlidar_ros2_ws/src
git clone https://github.com/ldrobotSensorTeam/ldlidar_ros2.git
cd ~/ldlidar_ros2_ws
colcon build
source install/local_setup.bash

# Cấp quyền cổng serial (hoặc thêm user vào group dialout cho lâu dài)
sudo chmod 777 /dev/ttyUSB0

ros2 launch ldlidar_ros2 ld14.launch.py

Driver đọc đúng giao thức 47 byte ở phần 3 (LD14 baud 115200 — sai cổng hay baud thì sửa port_name trong file launch) và publish topic scan kiểu sensor_msgs/LaserScan. Kiểm tra:

ros2 topic hz /scan     # ~6–10 Hz, đúng tốc độ quay của lidar
rviz2                   # add LaserScan, topic /scan

Repo có sẵn viewer_ld14.launch.py để chạy kèm rviz2 bằng một lệnh. Với hệ dùng Nav2 cần node kiểu lifecycle, tham khảo thêm Myzhar/ldrobot-lidar-ros2.

Phương án ESP32 ở các phần trên phù hợp cho thiết bị độc lập không có máy chạy ROS2: demo cầm tay, cảm biến gắn cố định, robot nhỏ.

Cấu trúc project

ld14_web/
├── CMakeLists.txt
├── sdkconfig.defaults
└── main/
    ├── CMakeLists.txt
    ├── main.c          ← lidar + web server, ~360 dòng
    └── index.html      ← trang radar, nhúng vào firmware

Toàn bộ code: github.com/dungtsbb/ld14_web (giấy phép MIT).