【嵌入式】探索基于ESPIDF的乐动LD14P激光雷达驱动与数据通信

CRC8校验与异或校验的优劣 🔥

异或校验是「轻量简易的入门校验」;CRC 校验是「工业级高可靠性的主流校验」

异或校验（简单异或和 / 校验和）

• 符号 ："^"（数字6那里）

• 实现原理：将16进制转为8为二进制后，对应位置按照相同 = 0，不同 = 1的规则，异或的结果再转为16进制

  0 ^ 0 = 0
  0 ^ 1 = 1
  1 ^ 0 = 1
  1 ^ 1 = 0

• 实现流程 ：例 0x01 ^ 0x02 = 0x03

  0x01 → 0000 0001
  0x02 → 0000 0010

  0000 0001

  ^ 0000 0010

  0000 0011

  0000 0011 → 0x03

• 代码实现:

uint8_t xor_check(uint8_t *data, uint8_t len)
{
    uint8_t check = 0;       // 初始值 = 0
    for(int i=0; i<len; i++)
    {
        check ^= data[i];    // 逐个异或
    }
    return check;
}

• 致命缺点：漏检率极高，完全不适合工业场景

在串口通信中，通信会受到电机电磁干扰、线材信号衰减、外界环境杂波 等影响，而异或校验在这些场景下几乎等于没校验

示例：

• 0x12,0x34，异或和=0x26，若传输中变成0x13,0x35（两个字节各翻 1 位），异或和=0x26，异或校验认为数据无误，实际数据已错
• 0x01,0x02变成0x02,0x01，字节错位，异或和不变，异或校验无法检测；

CRC 校验（循环冗余校验，以 CRC8 为例）

• 查表法 ：多项式(0x4d): x 6 + x 3 + x 2 + 1 x^6 + x^3 + x^2 + 1 x6+x3+x2+1

  x⁶ + x³ + x² + 1

  位7 位6 位5 位4 位3 位2 位1 位0
  0 1 0 0 1 1 0 1

  = 0100 1101
  = 0x4D

• CRC8数据表

static const uint8_t CRC_TABLE[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,
    0x8f, 0xd3, 0x9e, 0x49, 0x04, 0xaa, 0xe7, 0x30, 0x7d, 0x88, 0xc5, 0x12,
    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
};

为了提高计算 CRC 校验值的速度，通常会预先计算出所有可能的 256 个字节对应的校验结果并存入数组，这样在运行时只需通过"查表"即可完成计算，而不需要逐位进行复杂的异或（XOR）和移位操作。

• 数据生成方式 （模 2 除法 ：看的是最高位（最高次幂）是否对齐1。它不关心数值大小，只关心"位"。）

#include <stdio.h>

void generate_crc8_table() {
    unsigned char poly = 0x4d; // 生成多项式
    unsigned char table[256];

    for (int i = 0; i < 256; i++) {
        unsigned char crc = (unsigned char)i;
        for (int j = 0; j < 8; j++) {
            if (crc & 0x80) { //检查最高位是否为 1 (相当于除法的商为 1)意味着当前的数值已经达到了生成多项式的"量级"，商为 1。这时必须执行"减法"（即异或运算 ^ 0x4d）
                crc = (crc << 1) ^ poly;//移位并进行异或 (相当于减去除数)
            } else {
                crc <<= 1;//高位为 0，只需移位 (相当于商为 0)
            }
        }
        table[i] = crc;
    }

}

• 雷达数据实际应用

/**
 * @brief 雷达数据CRC校验
 * @param protocol_buf 串口发来的数据包
 * @param crc_len 数据包长度
 * @return CRC8校验值
 */
static uint8_t LD14P_Calculate_CRC8(uint8_t* protocol_buf, uint8_t crc_len)
{
    int i;
    uint8_t crc = 0x00;
    for (i = 0; i < crc_len; i++)
        crc = CRC_TABLE[(crc ^ protocol_buf[i]) & 0xFF]; //查表法，省去按位运算，提高性能,& 0xFF 是为了确保结果严格落在 0 到 255 的范围内。
    return crc;
}

查表法的 CRC8 表仅256 字节，CRC16 表是 65536 字节，对雷达的低配 MCU 来说，更节省内存。

激光雷达数据包格式

• 起始符：长度1 Byte，值固定为0x54，表示数据包的开始；

• VerLen(固定) ：长度1 Byte，高三位表示帧类型，目前固定为1，低五位表示一个包的测量点数，目前固定为12 ，故该字节值固定为0x2C；

• 雷达转速：长度2 Byte，单位为度每秒；

• 起始角度: 长度2 Byte，单位为0.01度；

• 数据 ：一个测量数据长度为3个字节，包括2字节的距离和1字节的强度，会有12个测量点

• 结束角度：长度2 Byte，单位为0.01度；

• 时间戳 ：长度2 Byte，单位为ms，最大为30000，到达30000会重新

  计数；

• CRC校验：前面所有数据的校验和

数据包结构：

固定长度 = 1(头)+1(VerLen)+2(转速)+2(起始角)+12*3(点数据)+2(结束角)+2(时间戳)+1(CRC) = 47 字节

激光雷达接收数据状态机

• 初始状态0：仅找LD14P唯一帧头0x54
• 核心状态1：接收点云包的所有剩余数据（1~46字节）

/**
 * @brief 状态机读取串口数据
 * @param pkg 串口发来的单字节
 * @return 无
 */
void LD14P_Data_Receive(uint8_t rx_data)
{
    static uint8_t rx_flag = 0;        // 0：初始状态（找帧头）; 1：收点云包数据
    static uint8_t rx_buf_len = 0;     // 数据包总长度
    static uint8_t rx_buf_index = 0;   // 当前接收字节索引
    
    switch (rx_flag)
    {
    case 0: // 初始状态：仅找LD14P唯一帧头0x54
    {
        if (rx_data == LD14P_HEAD) // 匹配LD14P帧头
        {
            rx_flag = 1; // 直接进入状态1：收点云包数据
            rx_protocol_buf[0] = LD14P_HEAD; // 帧头存入缓冲区0字节
            rx_buf_index = 1; // 下一个字节从索引1开始
            rx_buf_len = LD14P_PACKAGE_LEN; // LD14P固定包长47字节
        }
        break;
    }
    case 1: // 核心状态：接收点云包的所有剩余数据（1~46字节）
    {
        rx_protocol_buf[rx_buf_index] = rx_data; // 存入当前字节
        rx_buf_index++; // 索引自增，存入雷达核心数据
        
        // 激光雷达数据接收完成（一帧）（索引达到固定包长47）
        if (rx_buf_index >= rx_buf_len)
        {
            // 重置状态机，准备收下一包
            rx_flag = 0;
            rx_buf_len = 0;
            rx_buf_index = 0;
            // 解析点云包→拼接360°数据→置新包标志
            if (LD14P_Parse_Package(rx_protocol_buf, &ld14p_pkg) == ESP_OK)
            {
                LD14P_Output_Data(&ld14p_pkg, &ld14p_data);
                new_package = 1; // 通知上层有新数据
            }
            memset(rx_protocol_buf, 0, sizeof(rx_protocol_buf)); // 清空缓冲区
        }
        // 缓冲区溢出保护（防止内存越界）
        if (rx_buf_index >= LD14P_BUF_MAX)
        {
            rx_flag = 0;
            rx_buf_len = 0;
            rx_buf_index = 0;
            memset(rx_protocol_buf, 0, sizeof(rx_protocol_buf));
        }
        break;
    }
    default: // 异常状态：重置所有变量
        rx_flag = 0;
        rx_buf_len = 0;
        rx_buf_index = 0;
        break;
    }
}

解析激光雷达点云数据包🌟

LD14P激光雷达数据手册： LDROBOT_LD14P DataSheet_CN_v0.4_Wlmrp6QT.pdf

LD14P激光雷达开发手册： LDROBOT_LD14_Development_Manual_CN_v1_20220530170742.0.pdf

通信接口

ESPIDF配置串口通信

• 芯片：ESP32S3

• 串口号：UART_NUM_1

• RX ：GPIO17; TX：GPIO18

• 波特率:230400

串口初始化

const uart_config_t uart_config = {
    .baud_rate = 230400,//设置串口波特率为 230400，适配LD14P雷达
    .data_bits = UART_DATA_8_BITS,//数据位配置为 8 位
    .parity = UART_PARITY_DISABLE,//禁用奇偶校验
    .stop_bits = UART_STOP_BITS_1,//停止位配置为 1 位
    .flow_ctrl = UART_HW_FLOWCTRL_DISABLE,//禁用硬件流控（RTS/CTS）
    .source_clk = UART_SCLK_DEFAULT//使用默认时钟源
};
//安装 UART 驱动(参数):指定操作的串口号,UART 硬件接收缓冲区大小(字节),UART 硬件发送缓冲区大小（字节）,UART 事件队列长度,事件队列句柄,内存分配标志
uart_driver_install(UART_NUM_1, RX0_BUF_SIZE * 2, 0, 0, NULL, 0);
//配置 UART 参数(参数)：目标串口号,串口配置结构体的指针
uart_param_config(UART_NUM_1, &uart_config);
//绑定 UART 引脚(参数)：目标串口号，TX 引脚编号，RX 引脚编号，RTS 引脚，CTS 引脚：
uart_set_pin(UART_NUM_1, UART0_GPIO_TXD, UART0_GPIO_RXD, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
//初始化环形缓冲区
RingBuffer_Init(&uart1_ringbuf, 1024);
//FreeRTOS 接口，创建一个独立的任务来持续读取 UART0 硬件数据;(参数)：任务函数入口，任务名称，任务栈大小，任务参数，任务优先级，任务句柄
xTaskCreate(Uart1_Rx_Task, "Uart1_Rx_Task", 5*1024, NULL, 5, NULL);

数据包解析：

雷达发出来的是16 位数据 → 拆成 2 个 8 位字节发，低字节先发（LSB），高字节后发（MSB）

规则 ：(高字节 << 8) | 低字节

例如 ：真实转速 = 0x1234

• 低 8 位 = 0x34
• 高 8 位 = 0x12

高字节左移 8 位： 0x12 << 8 = 0x1200

低字节 或 上去： 0x1200 | 0x34 = 0x1234

0001 0010 << 8
= 0001 0010 0000 0000  → 0x1200

0001 0010 0000 0000 | 0000 0000 0011 0100
= 0001 0010 0011 0100 → 0x1234

雷达测量点角度与强度值解码

// 6~41字节：12个点，每个点3字节
for (int i = 0; i < LD14P_POINT_PER_PACK; i++)
{
    // 距离：低8位(3*i+6) + 高8位(3*i+7)，LSB在前（开发手册4.2节明确）
    out_pkg->points[i].distance = (protocol_buf[3*i+7] << 8) | protocol_buf[3*i+6];
    out_pkg->points[i].intensity = protocol_buf[3*i+8]; // 强度：3i+8字节
}

对于第一个测量点发来的雷达角度数据，高八位是数组的索引为7的位置

完整代码

/**
 * @brief 点云解析
 * @param protocol_buf 串口发来的数据包
 * @param out_pkg LD14P的数据包结构体指针
 * @return 执行是否成功
 */
static int LD14P_Parse_Package(uint8_t* protocol_buf, ld14p_package_t* out_pkg)
{
    // LD14P固定数据包长度47字节，CRC校验前46字节
    uint8_t buf_len = LD14P_PACKAGE_LEN;  //非固定点数要 (protocol_buf[1] & 0x1F) * 3 + 11;
    uint8_t check_num = protocol_buf[buf_len-1]; // 最后1字节是CRC8
    uint8_t crc8 = LD14P_Calculate_CRC8(protocol_buf, buf_len-1); // CRC计算

    // CRC校验失败，返回错误
    if (crc8 != check_num)
    {
        ESP_LOGI(TAG, "CRC Error:%d, %d", crc8, check_num);
        return ESP_FAIL;
    }

     // 按LD14P协议逐字节解析（开发手册4.1节，固定字节位！）
    out_pkg->header = protocol_buf[0];          // 0字节：帧头0x54
    out_pkg->ver_len = protocol_buf[1];         // 1字节：VerLen 0x2C
    out_pkg->speed = (protocol_buf[3] << 8) | protocol_buf[2]; // 2-3字节：转速（LSB在前，MSB在后）
    out_pkg->start_angle = (protocol_buf[5] << 8) | protocol_buf[4]; //4-5字节：起始角度

    // 6~41字节：12个点，每个点3字节
    for (int i = 0; i < LD14P_POINT_PER_PACK; i++)
    {
        // 距离：低8位(3*i+6) + 高8位(3*i+7)，LSB在前（开发手册4.2节明确）
        out_pkg->points[i].distance = (protocol_buf[3*i+7] << 8) | protocol_buf[3*i+6];
        out_pkg->points[i].intensity = protocol_buf[3*i+8]; // 强度：3i+8字节
    }

    out_pkg->end_angle = (protocol_buf[43] << 8) | protocol_buf[42]; //42-43字节：结束角度
    out_pkg->timestamp = (protocol_buf[45] << 8) | protocol_buf[44]; //44-45字节：时间戳
    out_pkg->crc8 = protocol_buf[46]; //46字节：CRC8校验位

    return ESP_OK;
}

提取协议包并更新雷达数据包

雷达工作流程：

• 雷达转一圈，发 30 包
• 每包 12 个点
• 每包自带 起始角度、结束角度
• 计算出每个点对应的 真实角度（0~359）
• 把 12 个点按角度贴回 360° 数组
• 30 包贴完 → 得到一圈完整雷达图

计算角度步长

• 角度递增情况(12个点11个间隔)

step_angle = (pkg->end_angle - pkg->start_angle) / (LD14P_POINT_PER_PACK - 1); // 固定12点

• 角度递减情况(360~0)

step_angle = (36000 + pkg->end_angle - pkg->start_angle) / (LD14P_POINT_PER_PACK - 1);

映射每个点到0~359°数组

for (int i = 0; i < LD14P_POINT_PER_PACK; i++)
{
    // 计算实际角度：0.01°→1°，模360防止越界
    angle = ((pkg->start_angle + i * step_angle) / 100) % LD14P_POINT_MAX;
    out_data->points[angle].distance = pkg->points[i].distance;
    out_data->points[angle].intensity = pkg->points[i].intensity;
}

模360：把任何角度强制拉回 0~359

370 % 360 = 10
400 % 360 = 40
720 % 360 = 0

/100：雷达单位为0.01度，除以一百变为度

雷达坐标系转换为ROS坐标系

传感器的正前方定义为零度 方向，旋转角度沿着顺时针方向增大，

LD14P传
小车雷达硬件安装方向

修改映射为小车ros坐标系

    for (int i = 0; i < LD14P_POINT_PER_PACK; i++)
    {
        // 计算实际角度：0.01°→1°，模360防止越界
        // angle = ((pkg->start_angle + i * step_angle) / 100) % LD14P_POINT_MAX;
        int real_angle = pkg->start_angle + i * step_angle;
        //修正坐标系为小车ROS坐标系( 18000 - real_angle 会让很多角度直接变成 0 或负数,所以多加一圈w为54000)
        angle = ((54000 - real_angle) / 100) % LD14P_POINT_MAX; //模360把任何角度强制拉回 0~359
        if(angle < 0) angle += 360;

        out_data->points[angle].distance = pkg->points[i].distance;
        out_data->points[angle].intensity = pkg->points[i].intensity;
    }

激光雷达数据解析任务

/**
 * @brief 激光雷达解析数据任务
 * @param arg 任务传的参数
 * @return 无
 */
static void Lidar_LD14P_Task(void *arg)
{
    ESP_LOGI(TAG, "Start Lidar_LD14P_Task with core:%d", xPortGetCoreID());
    uint16_t rx_count = 0;
    
    while (1)
    {
        rx_count = Uart1_Available();
        if (rx_count)
        {
            // Uart1_Clean_Buffer();
            for (int i = 0; i < rx_count; i++)
                LD14P_Data_Receive(Uart1_Read());
        }
        if (LD14P_New_Package())
            LD14P_Clear_New_Package_State();
            // LD14P_Get_Data(&lidar_data);
        else
        {
            vTaskDelay(pdMS_TO_TICKS(1));
        }
    }
    vTaskDelete(NULL);
}

主函数

void app_main(void)
{
    Uart1_Init();
    Lidar_LD14P_Init();
    vTaskDelay(pdMS_TO_TICKS(1000));

    uint16_t points[4] = {0, 90, 180, 270};
    uint16_t distances[4] = {0};
    while (1)
    {
        for (int i = 0; i < 4; i++)
            distances[i] = Lidar_LD14P_Get_Distance(points[i]);
        ESP_LOGI(TAG, "distance:%d, %d, %d, %d", distances[0], distances[1], distances[2], distances[3]); //单位mm
        vTaskDelay(pdMS_TO_TICKS(200));
    }
}

输出

void app_main(void)
{
    Uart1_Init();
    Lidar_LD14P_Init();
    vTaskDelay(pdMS_TO_TICKS(1000));

    uint16_t points[4] = {0, 90, 180, 270};
    uint16_t distances[4] = {0};
    while (1)
    {
        for (int i = 0; i < 4; i++)
            distances[i] = Lidar_LD14P_Get_Distance(points[i]);
        ESP_LOGI(TAG, "distance:%d, %d, %d, %d", distances[0], distances[1], distances[2], distances[3]); //单位mm
        vTaskDelay(pdMS_TO_TICKS(200));
    }
}