[STM32]手持建图仪--基于STM32F103+思岚A1激光雷达的扫描仪

前言
一个朋友在做服务机器人项目，用到思岚的激光雷达，于是便把淘汰的A1M8雷达送我一个，本着拿到啥就玩啥的态度，必须整一波。其实激光雷达还是搭配ROS才能发挥最大的作用，奈何资源有限，实力不足，只能依靠STM32开发板做一个及其简陋的地图扫描。
思岚A1M8激光雷达

这款激光雷达属于低成本的360度激光扫描测距雷达，外置电机，使用皮带带动雷达转台转动，实现360度的测距扫描，电机的转速由MCU发送PWM控制。
外部系统通过 TTL 电平的 UART 串口信号与 RPLIDAR 测距核心进行通讯。通过本文档定义的通讯协议，外部系统可以实时获取 RPLIDAR 的扫描数据、设备信息、设备健康状态。并且通过相关命令调整 RPLIDAR 的工作模式。
按照不同的请求类型， RPLIDAR 具有三种不同的请求/应答模式：
标准的单次请求-单次应答模式

单次请求-多次应答模式

单次请求/无应答模式

对于停止扫描、重启测距核心这类请求命令， RPLIDAR 采用单次请求，但不做应答的通讯模式。此时外部系统需要在发送请求后等待一定的时间，待RPLIDAR 完成了上一次请求操作后方可继续执行下一次请求。否则第二次的请求将可能被 RPLIDAR 丢弃。
在此次应用中，主要采用后两种请求/应答模式，使用单次请求-多次应答模式采集测距数据，使用单次请求/无应答模式停止采样，进行数据的处理。
在单次请求-多次应答模式采集测距数据时，MCU发送采集指令，雷达会先回复一条起使应答报文，之后便会循环回复数据应答报文。

请求报文及起始应答数据格式如下：

在回复起始应答之后，雷达会循环回复测距数据。长度为5bytes.
 
例如测距数据为 3E D5 16 77 06。
第一个字节：3E，二进制为：0011 1110。代表信号质量为0x0f。信号质量不为零代表数据有效，起始标志位为0，代表不是新的一圈，该标志位只有在新的一圈的第一帧数据才会置一，该圈内的其余数据改为依旧是0。
第二个字节：D5，角度数据低七位。
第三个字节：16，角度数据高八位，加上第二个字节的低七位等于166A，再右移一位得B35。实际角度=835/64=44°，该角度表示与雷达零度的顺时针偏移角度，如下图。
第四个字节：77，距离数据低八位。
第五个字节：06，距离角度高八位。则此时距离为0x0677/4 = 413mm。

激光雷达测试：
接线：
雷达             MCU
GND----------->GND
RX------------->TX
TX------------->RX
V5.0----------->5V
GND----------->GND
MOTOCTL---->PWM
VMOTO------->5V
首先测试使用串口助手进行数据采集，这里将MOTOCTL接到5V电源，直接以最高速度进行采样。串口助手发送A5 20,可以看到数据滚动。

其中开头的七位数据对应起始应答，后面每5个字节一组，对应测距数据。雷达无损坏，开始连接开发板调试。
MCU代码：
既然是USART通信，我们先初始化USART，使用串口接收中断接收数据。

void USART_Config(void)

{

        GPIO_InitTypeDef GPIO_InitStructure;

        USART_InitTypeDef USART_InitStructure;

        NVIC_InitTypeDef NVIC_InitStructure;

        // 打开串口GPIO的时钟

        DEBUG_USART_GPIO_APBxClkCmd(DEBUG_USART_GPIO_CLK, ENABLE);

        

        // 打开串口外设的时钟

        DEBUG_USART_APBxClkCmd(DEBUG_USART_CLK, ENABLE);



        // 将USART Tx的GPIO配置为推挽复用模式

        GPIO_InitStructure.GPIO_Pin = DEBUG_USART_TX_GPIO_PIN;

        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;

        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

        GPIO_Init(DEBUG_USART_TX_GPIO_PORT, &GPIO_InitStructure);



  // 将USART Rx的GPIO配置为浮空输入模式

        GPIO_InitStructure.GPIO_Pin = DEBUG_USART_RX_GPIO_PIN;

        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;

        GPIO_Init(DEBUG_USART_RX_GPIO_PORT, &GPIO_InitStructure);

        

        //Usart1 NVIC 配置

        NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;

        NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=2 ;//抢占优先级3

        NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;                //子优先级3

        NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;                        //IRQ通道使能

        NVIC_Init(&NVIC_InitStructure);        //根据指定的参数初始化VIC寄存器

        

        // 配置串口的工作参数

        // 配置波特率

        USART_InitStructure.USART_BaudRate = DEBUG_USART_BAUDRATE;

        // 配置 针数据字长

        USART_InitStructure.USART_WordLength = USART_WordLength_8b;

        // 配置停止位

        USART_InitStructure.USART_StopBits = USART_StopBits_1;

        // 配置校验位

        USART_InitStructure.USART_Parity = USART_Parity_No ;

        // 配置硬件流控制

        USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;

        // 配置工作模式，收发一起

        USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;

        // 完成串口的初始化配置

        USART_Init(DEBUG_USARTx, &USART_InitStructure);



        USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//开启接收中断

        USART_ClearFlag(USART1,USART_FLAG_TC|USART_FLAG_RXNE);

//    USART_DMACmd(USART1, USART_DMAReq_Rx, ENABLE);  // 开启串口DMA接收

        // 使能串口

        USART_Cmd(DEBUG_USARTx, ENABLE);            

}

然后编写中断服务函数

void USART1_IRQHandler(void)                        //串口1中断服务程序

{



        if(USART_GetITStatus(DEBUG_USARTx,USART_IT_RXNE)!=RESET)

        {

                rxbuff[Res] = USART_ReceiveData(DEBUG_USARTx);

                Res++;

                if(Res==1807)

                {

                        USART_ITConfig(USART1, USART_IT_RXNE, DISABLE);//开启接收中断

                        USART_SendData(USART1,0xA5);

                        while (USART_GetFlagStatus(DEBUG_USARTx, USART_FLAG_TXE) == RESET);        

                        USART_SendData(USART1,0x25);

                        while (USART_GetFlagStatus(DEBUG_USARTx, USART_FLAG_TXE) == RESET);

                        Data_Processing();

                        Res=0;

                        ClearFlag=1;

                }

//                MYDMA_Enable(DMA1_Channel5);//开始一次DMA传输！

        }

}

在串口中断服务函数中，需要采集1807个数据（360个测距点*5字节+起始7个字节）。我采用全速采样，即MOTOCTL直接接5V，这里采集360个数据点其实不止一圈的数据，但是因为每个360度都有无效数据，多采集点可以使后期画图更完整。在提取数据使用EXCEL分析以后，全速转一圈大概采样258个点左右，这个数据无法固定，每一圈采样数均不一样。
在采集数据完成后我们需要关闭采样，因为STM32F103的数据处理能力并不理想，这里需要一定的时间，于是通过串口发送指令A5 25让雷达停止采样，同时调用函数Data_Processing();进行数据处理以及在屏幕上画点。这里要注意，雷达在停止采样前会将最后一帧数据发送完整，我们在发送停止指令的期间，雷达可能已经在准备下一帧数据，在发送完停止指令之后，可能会存在这一帧数据的最后一位未触发中断，但是串口的数据寄存器中已经保存了这位数据，且已经改变了标志位，所以在下一次启动采样时会导致收到的第一个数据是上一次未接收完的数据。这个在进行处理。
在此之前我们还需要一个触发采样的按键。按下按键后触发采样，为了保持持续采样，在串口接收中断关闭采样并处理完数据后，可在主循环中再次开启。

void KEY1_IRQHandler(void)

{

        u8 RX;

  //确保是否产生了EXTI Line中断

        if(EXTI_GetITStatus(KEY1_INT_EXTI_LINE) != RESET) 

        {

        

                USART_SendData(USART1,0xA5);

                while (USART_GetFlagStatus(DEBUG_USARTx, USART_FLAG_TXE) == RESET);        

                USART_SendData(USART1,0x20);

                while (USART_GetFlagStatus(DEBUG_USARTx, USART_FLAG_TXE) == RESET);                        

                USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//开启空闲中断

                Res=0;

    //清除中断标志位

                EXTI_ClearITPendingBit(KEY1_INT_EXTI_LINE);     

        }  

}

数据处理如下：

void Data_Processing(void)

{

        u16 i,j=7;

        u8 quality;



        for(i=0;i<360;i++)

        {

                quality = rxbuff[j]>>2;

                if(quality!=0)

                {

                        data_rage1 = rxbuff[j+2]<<8;

                        data_rage2 = rxbuff[j+1];

                        angle[i] = (data_rage1 | data_rage2)>>1;

                        angle[i] = angle[i];

                        data_rage1 = rxbuff[j+4]<<8;

                        data_rage2 = rxbuff[j+3];

                        distance[i] = (data_rage1|data_rage2);        

//                Usart_SendHalfWord(USART2,angle[i]);        

//                Usart_SendHalfWord(USART2,distance[i]);        

                }

                

                j = j+5;

        }



        if(i==360) 

        {

                LCD_Draw();

                i=0;

//                

        }

}

从串口缓存数组中取出角度值和距离值，保存在数组angle[]和distance[]中。当360个数据点处理完，调用画图函数进行屏幕绘制。

void LCD_Draw(void)

{

        u16 i;

         ILI9341_Clear(0,0,LCD_X_LENGTH,LCD_Y_LENGTH);        /* 清屏，显示全黑 */

        LCD_SetTextColor(RED);

        for(i=0;i<360;i++)

        {

                x=return_x(angle[i], distance[i]/scale);

                y=return_y(angle[i], distance[i]/scale);

//                ILI9341_DrawLine(120,160,x,y);

                ILI9341_SetPointPixel(x,y);

                /*为了点更清楚，在点周围画辅助点*/

                ILI9341_SetPointPixel(x+1,y+1);

                ILI9341_SetPointPixel(x-1,y-1);

                ILI9341_SetPointPixel(x-1,y+1);

                ILI9341_SetPointPixel(x+1,y-1);

                ILI9341_SetPointPixel(x+2,y+2);

                ILI9341_SetPointPixel(x-2,y-2);

                ILI9341_SetPointPixel(x-2,y+2);

                ILI9341_SetPointPixel(x+2,y-2);                

        }        

}

画点直接调用野火的库，其中参数scale为地图放大倍数，因为屏幕大小有限，为了适应不同大小的地图，使用该参数进行地图放大。
return_x，return_y函数是将测距点转换为屏幕坐标。原函数如下：

//x坐标转换函数

//ang:0~359度数,    d:距离

//返回：x坐标0~239

float return_x(u16 ang, signed int d)

{

        float x;

        double ang_deg,dd;

        ang_deg = ang/64;

        dd = d/4;

        if(dd!=0)

        {

                if(ang_deg <= 90)

                {

                        

                        x = dd*sin(ang_deg)+120;//角度转换成弧度

                }

                else if((ang_deg > 90) && (ang_deg <= 180))

                {

                        

                        x = 120+dd*sin(ang_deg);

                }

                else if((ang_deg > 180) && (ang_deg <= 270))

                {



                        x = 120-dd*sin(ang_deg);

                }

                else if((ang_deg > 270) && (ang_deg <= 359))

                {



                        x = 120-dd*sin(ang_deg);

                }        

        

        }



        if(x > 239)

                x = 239;

        if(x < 0)

                x = 0;

        return x;

}

//y坐标转换函数

//ang:0~359度数,    d:距离

//返回：y坐标0~319

float return_y(u16 ang, signed int d)

{

        float y,dd;

        double ang_deg;

        ang_deg = ang/64;

        dd = d/4;

        if(dd!=0)

        {

                if(ang_deg <= 90)

                {



                        y = 160-dd*cos(ang_deg);//角度转换成弧度

                }

                else if((ang_deg > 90) && (ang_deg <= 180))

                {

                

                        y = dd*cos(ang_deg)+160;

                }

                else if((ang_deg > 180) && (ang_deg <= 270))

                {



                        y = dd*cos(ang_deg)+160;

                }

                else if((ang_deg > 270) && (ang_deg <= 359))

                {

                

                        y = 160-dd*cos(ang_deg);

                }        

        }



        if(y > 319)

                y = 319;

        if(y < 0)

                y = 0;

        return y;

}
此时在屏幕上便可绘制出雷达采样点

 
这里是动态监测的，但是动态图在后面补，后续也会优化绘图和数据处理，这里先给出大致的效果。时间有限，目前先这样，后面会完善此贴。
---------------------
作者：呐咯密密
链接：https://bbs.21ic.com/icview-3169632-1-1.html
来源：21ic.com
此文章已获得原创/原创奖标签，著作权归21ic所有，任何人未经允许禁止转载