前言
一個朋友在做服務(wù)機(jī)器人項目,用到思嵐的激光雷達(dá),于是便把淘汰的A1M8雷達(dá)送我一個,本著拿到啥就玩啥的態(tài)度,必須整一波。其實激光雷達(dá)還是搭配ROS才能發(fā)揮最大的作用,奈何資源有限,實力不足,只能依靠STM32開發(fā)板做一個及其簡陋的地圖掃描。
思嵐A1M8激光雷達(dá)
這款激光雷達(dá)屬于低成本的360度激光掃描測距雷達(dá),外置電機(jī),使用皮帶帶動雷達(dá)轉(zhuǎn)臺轉(zhuǎn)動,實現(xiàn)360度的測距掃描,電機(jī)的轉(zhuǎn)速由MCU發(fā)送PWM控制。
外部系統(tǒng)通過 TTL 電平的 UART 串口信號與 RPLIDAR 測距核心進(jìn)行通訊。通過本文檔定義的通訊協(xié)議,外部系統(tǒng)可以實時獲取 RPLIDAR 的掃描數(shù)據(jù)、設(shè)備信息、設(shè)備健康狀態(tài)。并且通過相關(guān)命令調(diào)整 RPLIDAR 的工作模式。
按照不同的請求類型, RPLIDAR 具有三種不同的請求/應(yīng)答模式:
標(biāo)準(zhǔn)的單次請求-單次應(yīng)答模式
單次請求-多次應(yīng)答模式
單次請求/無應(yīng)答模式
對于停止掃描、重啟測距核心這類請求命令, RPLIDAR 采用單次請求,但不做應(yīng)答的通訊模式。此時外部系統(tǒng)需要在發(fā)送請求后等待一定的時間,待RPLIDAR 完成了上一次請求操作后方可繼續(xù)執(zhí)行下一次請求。否則第二次的請求將可能被 RPLIDAR 丟棄。
在此次應(yīng)用中,主要采用后兩種請求/應(yīng)答模式,使用單次請求-多次應(yīng)答模式采集測距數(shù)據(jù),使用單次請求/無應(yīng)答模式停止采樣,進(jìn)行數(shù)據(jù)的處理。
在單次請求-多次應(yīng)答模式采集測距數(shù)據(jù)時,MCU發(fā)送采集指令,雷達(dá)會先回復(fù)一條起使應(yīng)答報文,之后便會循環(huán)回復(fù)數(shù)據(jù)應(yīng)答報文。
請求報文及起始應(yīng)答數(shù)據(jù)格式如下:
在回復(fù)起始應(yīng)答之后,雷達(dá)會循環(huán)回復(fù)測距數(shù)據(jù)。長度為5bytes。
例如測距數(shù)據(jù)為 3E D5 16 77 06。
第一個字節(jié):3E,二進(jìn)制為:0011 1110。代表信號質(zhì)量為0x0f。信號質(zhì)量不為零代表數(shù)據(jù)有效,起始標(biāo)志位為0,代表不是新的一圈,該標(biāo)志位只有在新的一圈的第一幀數(shù)據(jù)才會置一,該圈內(nèi)的其余數(shù)據(jù)改為依舊是0。
第二個字節(jié):D5,角度數(shù)據(jù)低七位。
第三個字節(jié):16,角度數(shù)據(jù)高八位,加上第二個字節(jié)的低七位等于166A,再右移一位得B35。實際角度=835/64=44°,該角度表示與雷達(dá)零度的順時針偏移角度,如下圖。
第四個字節(jié):77,距離數(shù)據(jù)低八位。
第五個字節(jié):06,距離角度高八位。則此時距離為0x0677/4 = 413mm。
激光雷達(dá)測試:
接線:
雷達(dá) MCU
GND----------->GND
RX------------->TX
TX------------->RX
V5.0----------->5V
GND----------->GND
MOTOCTL---->PWM
VMOTO------->5V
首先測試使用串口助手進(jìn)行數(shù)據(jù)采集,這里將MOTOCTL接到5V電源,直接以最高速度進(jìn)行采樣。串口助手發(fā)送A5 20,可以看到數(shù)據(jù)滾動。
其中開頭的七位數(shù)據(jù)對應(yīng)起始應(yīng)答,后面每5個字節(jié)一組,對應(yīng)測距數(shù)據(jù)。雷達(dá)無損壞,開始連接開發(fā)板調(diào)試。
MCU代碼:
既然是USART通信,我們先初始化USART,使用串口接收中斷接收數(shù)據(jù)。
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);
// 打開串口外設(shè)的時鐘
DEBUG_USART_APBxClkCmd(DEBUG_USART_CLK, ENABLE);
// 將USART Tx的GPIO配置為推挽復(fù)用模式
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 ;//搶占優(yōu)先級3
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3; //子優(yōu)先級3
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道使能
NVIC_Init(&NVIC_InitStructure); //根據(jù)指定的參數(shù)初始化VIC寄存器
// 配置串口的工作參數(shù)
// 配置波特率
USART_InitStructure.USART_BaudRate = DEBUG_USART_BAUDRATE;
// 配置 針數(shù)據(jù)字長
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;
// 配置工作模式,收發(fā)一起
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);
}
然后編寫中斷服務(wù)函數(shù):
void USART1_IRQHandler(void) //串口1中斷服務(wù)程序
{
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傳輸!
}
}
在串口中斷服務(wù)函數(shù)中,需要采集1807個數(shù)據(jù)(360個測距點*5字節(jié)+起始7個字節(jié))。我采用全速采樣,即MOTOCTL直接接5V,這里采集360個數(shù)據(jù)點其實不止一圈的數(shù)據(jù),但是因為每個360度都有無效數(shù)據(jù),多采集點可以使后期畫圖更完整。在提取數(shù)據(jù)使用EXCEL分析以后,全速轉(zhuǎn)一圈大概采樣258個點左右,這個數(shù)據(jù)無法固定,每一圈采樣數(shù)均不一樣。
在采集數(shù)據(jù)完成后我們需要關(guān)閉采樣,因為STM32F103的數(shù)據(jù)處理能力并不理想,這里需要一定的時間,于是通過串口發(fā)送指令A(yù)5 25讓雷達(dá)停止采樣,同時調(diào)用函數(shù)Data_Processing();進(jìn)行數(shù)據(jù)處理以及在屏幕上畫點。這里要注意,雷達(dá)在停止采樣前會將最后一幀數(shù)據(jù)發(fā)送完整,我們在發(fā)送停止指令的期間,雷達(dá)可能已經(jīng)在準(zhǔn)備下一幀數(shù)據(jù),在發(fā)送完停止指令之后,可能會存在這一幀數(shù)據(jù)的最后一位未觸發(fā)中斷,但是串口的數(shù)據(jù)寄存器中已經(jīng)保存了這位數(shù)據(jù),且已經(jīng)改變了標(biāo)志位,所以在下一次啟動采樣時會導(dǎo)致收到的第一個數(shù)據(jù)是上一次未接收完的數(shù)據(jù)。這個在進(jìn)行處理。
在此之前我們還需要一個觸發(fā)采樣的按鍵。按下按鍵后觸發(fā)采樣,為了保持持續(xù)采樣,在串口接收中斷關(guān)閉采樣并處理完數(shù)據(jù)后,可在主循環(huán)中再次開啟。
void KEY1_IRQHandler(void)
{
u8 RX;
//確保是否產(chǎn)生了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;
//清除中斷標(biāo)志位
EXTI_ClearITPendingBit(KEY1_INT_EXTI_LINE);
}
}
數(shù)據(jù)處理如下:
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;
//
}
}
從串口緩存數(shù)組中取出角度值和距離值,保存在數(shù)組angle[]和distance[]中。當(dāng)360個數(shù)據(jù)點處理完,調(diào)用畫圖函數(shù)進(jìn)行屏幕繪制。
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);
}
}
畫點直接調(diào)用野火的庫,其中參數(shù)scale為地圖放大倍數(shù),因為屏幕大小有限,為了適應(yīng)不同大小的地圖,使用該參數(shù)進(jìn)行地圖放大。
return_x,return_y函數(shù)是將測距點轉(zhuǎn)換為屏幕坐標(biāo)。原函數(shù)如下:
//x坐標(biāo)轉(zhuǎn)換函數(shù)
//ang:0~359度數(shù), d:距離
//返回:x坐標(biāo)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;//角度轉(zhuǎn)換成弧度
}
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坐標(biāo)轉(zhuǎn)換函數(shù)
//ang:0~359度數(shù), d:距離
//返回:y坐標(biāo)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);//角度轉(zhuǎn)換成弧度
}
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;
}
此時在屏幕上便可繪制出雷達(dá)采樣點
這里是動態(tài)監(jiān)測的,但是動態(tài)圖在后面補(bǔ),后續(xù)也會優(yōu)化繪圖和數(shù)據(jù)處理,這里先給出大致的效果。時間有限,目前先這樣,后面會完善此貼。
從正文可以看出該屏幕的顯示的掃描地圖是圓形,但是我的房間卻不是圓的。這個地圖明顯是有問題。但是無論無如何調(diào)整算法,顯示到屏幕上的測距點總是不正確。分析得出大概問題是出在屏幕上,因為屏幕分辨率有限,測的的尺寸為了能在屏幕上顯示,不得已將尺寸縮小幾十倍,導(dǎo)致數(shù)據(jù)嚴(yán)重失真。于是我將測距數(shù)據(jù)導(dǎo)出研究。此次用已知大小的物料箱將雷達(dá)倒扣在里面。物料箱的尺寸大約為36cm*45cm。手頭沒有卷尺,用一個小尺子量的,所以只是大概值。
雷達(dá)位于箱子中間,那么到最短到箱壁兩邊的距離大概是18和22.5厘米。
測試開始:
使用串口二將原始角度和距離值打印到串口助手:
使用world文檔將數(shù)據(jù)整理:
然后復(fù)制數(shù)據(jù)到excle,進(jìn)行數(shù)據(jù)處理,將角度和距離分別提取;
根據(jù)真實角度值選取一整圈距離數(shù)據(jù)(mm),插入雷達(dá)圖:
此圖因為有無效點,取出零點以及錯誤點后得到如下圖。
可以看到此時的雷達(dá)圖很接近我們的箱子真實形狀,距離大小也符合箱子尺寸。此時才可以算作成功,雖然屏幕任然無法完整顯示掃描地圖,但是數(shù)據(jù)的處理并無問題,單片機(jī)速度跟不上,屏幕分辨率也不夠,難受啊。
原文標(biāo)題:帥小伙自制手持建圖儀!基于STM32F103+思嵐A1激光雷達(dá)
文章出處:【微信公眾號:硬件攻城獅】歡迎添加關(guān)注!文章轉(zhuǎn)載請注明出處。
-
STM32
+關(guān)注
關(guān)注
2263文章
10847瀏覽量
353797 -
開發(fā)板
+關(guān)注
關(guān)注
25文章
4838瀏覽量
96857 -
激光雷達(dá)
+關(guān)注
關(guān)注
967文章
3891瀏覽量
189205
原文標(biāo)題:帥小伙自制手持建圖儀!基于STM32F103+思嵐A1激光雷達(dá)
文章出處:【微信號:mcu168,微信公眾號:硬件攻城獅】歡迎添加關(guān)注!文章轉(zhuǎn)載請注明出處。
發(fā)布評論請先 登錄
相關(guān)推薦
評論