QEA课程最终的项目,是利用激光雷达,和stm32制作检测物体形状,并据此规划路径,让小车抵达制定形状物体。地图类似下图。
首先呢,就是要用stm32读取到激光雷达的数据,再建图,识别形状,规划路径。
由于课程要求只能用stm32,不能用树莓派等更高性能的上位机,让项目难度陡增。
我们使用到的是思岚A1M8雷达。
从思岚官网得知到一些资料;首先,激光雷达采用串口协议进行通讯,波特率115200,8位字长,1位停止位,无校验,数据格式如下
可见,每个点的信息由五个字节的数据确定,第一个字节代表采样点的质量,2、3字节表示点的角度,4、5字节代表点的距离。
于是我们可以首先配置一下stm32的串口
usart.c
u16 angle2;
u16 distance;
void usart2_init(u32 bound2)
{
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE); //使能GPIOA时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2,ENABLE); //使能USART2
USART_DeInit(USART2);
//USART2_TX GPIOA.2
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2; //PA.2
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出
GPIO_Init(GPIOA,&GPIO_InitStructure);//初始化GPIOA.2
//USART2_RX GPIOA.3
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3; //PA.3
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;//浮空输入
GPIO_Init(GPIOA,&GPIO_InitStructure); //初始化GPIOA.3
//Usart2 NVIC 配置
NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3 ; //抢占优先级3
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3; //子优先级3
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道使能
NVIC_Init(&NVIC_InitStructure); //根据指定的参数初始化VIC寄存器
//USART 初始化设置
USART_InitStructure.USART_BaudRate = bound2;//串口波特率
USART_InitStructure.USART_WordLength = USART_WordLength_8b;//字长为8位数据格式
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(USART2,&USART_InitStructure); //初始化串口2
USART_ITConfig(USART2,USART_IT_RXNE, ENABLE);//开启串口接受中断
USART_Cmd(USART2,ENABLE); //使能串口2
}
usart.h
#ifndef __USARTT_H
#define __USARTT_H
#include "sys.h"
#include "stdlib.h"
void usart2_init(u32 bound2);
void UART_TX(void);
#endif
我们还需要通过串口发送a5 20两个字节给激光雷达,激光雷达接收到后,才会发送串口数据。以下为启动激光雷达代码
void UART_TX(void) //雷达启动
{
USART_ClearFlag(USART2,USART_FLAG_TC);
if(1)
{ USART_SendData(USART2,0xA5); //从串口2发送开始指令 USART_FLAG_TC: 发送移位寄存器发送完成标志位,全部发送完毕会置 1
while(USART_GetFlagStatus(USART2,USART_FLAG_TC)!=SET);//等待发送结束
USART_SendData(USART2,0x20); //从串口2发送结束指令
while(USART_GetFlagStatus(USART2,USART_FLAG_TC)!=SET);//等待发送结束
}
}
那么我们现在可以试着输出一下串口数据乐
u8 RX_buffer[4]={0};
char m;
int flag=0;
int count=0;
void USART2_IRQHandler(void) //串口2中断服务程序
{
if(USART_GetITStatus(USART2, USART_IT_RXNE) != RESET) //接收到数据
{
m=USART_ReceiveData(USART2);
printf("%c",m);
}
}
通过串口工具,我们得到以下数据
现在问题就出现了,由于激光雷达发送数据实在太快了,频繁让单片机进入中断,单片机由于算力不够,会漏掉一些数据。
那么如何解决呢,网上有些帖子说可以延时来降低处理数据密度,但我认为是不行的,延时后,就不知道连续的5个字节来确定一个点了。
我看了激光雷达传回来的16进制数据,经过大量数据的分析,发现,在我当前所处的环境下,5个字节中的第一个,也就是代表数据质量的字节,绝大多数是02和3E,那么我就可以通过检索02或者3E,来判断连续的五个点的信息。(此处需要大家亲自实验,不同环境下质量字节不一样,我只是根据当前环境来确定的02和3E)
然后就是解算数据乐
可以看到,当我们发送a5 20启动激光雷达时,激光雷达先会返回七位起始报文,然后再是一直输出点的串口信息。解算代码如下
u8 RX_buffer[4]={0};
char m;
int flag=0;
int count=0;
void USART2_IRQHandler(void) //串口2中断服务程序
{
if(USART_GetITStatus(USART2, USART_IT_RXNE) != RESET) //接收到数据
{
m=USART_ReceiveData(USART2);
//printf("%c",m);
if(flag==1)
{
RX_buffer[count]=m;
count++;
if(count==4)//开始解算数据
{
angle2=(RX_buffer[1]<<1);
distance=(RX_buffer[3]<<6|RX_buffer[2]>>2);
if(angle2!=0&&distance!=0)
{
printf("distance=%u\n",distance);
printf("\r\n");
printf("angle=%u\n",angle2);
printf("\r\n");
}
flag=0;//点的检测标志归位
count=0;//计数归0
}
}
if(flag==0)//点的检测标志
{
if(m==0x3E)//检测到点
{
flag=1;
}
}
}
}
然后通过串口工具就可以看到如下输出乐
关于激光雷达的布线也很简单
注意5V供电,然后MOTOCTL可以通过输入PWM来控制激光雷达的转速
接下来就是物体形状的检测啦,我将会在下一篇帖子更新。