前言

一個朋友在做服務機器人項目，用到思嵐的激光雷達，于是便把淘汰的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~239float 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~319float 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;}

此時在屏幕上便可繪制出雷達采樣點

這里是動態監測的，但是動態圖在后面補，后續也會優化繪圖和數據處理，這里先給出大致的效果。時間有限，目前先這樣，后面會完善此貼。

從正文可以看出該屏幕的顯示的掃描地圖是圓形，但是我的房間卻不是圓的。這個地圖明顯是有問題。但是無論無如何調整算法，顯示到屏幕上的測距點總是不正確。分析得出大概問題是出在屏幕上，因為屏幕分辨率有限，測的的尺寸為了能在屏幕上顯示，不得已將尺寸縮小幾十倍，導致數據嚴重失真。于是我將測距數據導出研究。此次用已知大小的物料箱將雷達倒扣在里面。物料箱的尺寸大約為36cm*45cm。手頭沒有卷尺，用一個小尺子量的，所以只是大概值。

雷達位于箱子中間，那么到最短到箱壁兩邊的距離大概是18和22.5厘米。

測試開始：

使用串口二將原始角度和距離值打印到串口助手：

使用world文檔將數據整理：

然后復制數據到excle，進行數據處理，將角度和距離分別提取;

根據真實角度值選取一整圈距離數據（mm），插入雷達圖：

此圖因為有無效點，取出零點以及錯誤點后得到如下圖。

可以看到此時的雷達圖很接近我們的箱子真實形狀，距離大小也符合箱子尺寸。此時才可以算作成功，雖然屏幕任然無法完整顯示掃描地圖，但是數據的處理并無問題，單片機速度跟不上，屏幕分辨率也不夠，難受啊。

原文標題：帥小伙自制手持建圖儀！基于STM32F103+思嵐A1激光雷達