思岚RPLIDAR A1激光雷达扫描测距的模式C语言实现（STM32）

该产品激光雷达单次请求-多次应答模式对应着扫描测距模式，也是获取数据的环节。

该通讯模式用于RPLIDAR 进行扫描测距的模式下。外部系统在发送开始扫描 的请求后，RPLIDAR 将开始连续的扫描测距。在每次测距操作完成后，对应 的测距采样点的信息（距离、角度等）将通过一个独立应答包的形式发送外部系统。在这个模式下，外部系统只需要发送单次的请求，并开始连续接受来自 RPLIDAR 的多个应答数据文报。其通讯模式如下图1所示：

图1、单次请求-多次应答模式

单次请求-多次应答模式为开始扫描采样(SCAN)命令，通讯格式如图2所示:

图2、 开始扫描采样命令请求及应答

其数据应答通讯时序如图3所示：

图3、数据应答通讯时序图

RPLIDAR在扫描测据中会将每个采样点通过上述结构的数据应答报文发送至
外部系统。S扫描起始标志位，S=1表示新的一圈 360 度扫描 的开始，扫描起始标志位的取反，Quality为信号质量，C为校验位始终为1，angle为测距点相对于 RPLIDAR 朝向夹角，实际角度为angle/64，distance为测距点相对于 RPLIDAR 的距离实际距离为distance/4。其中开始扫描采样命令请求程序设计如下所示：

/*********开始扫描采样命令请求程序**********/
/*********开始扫描**********/

void Start_Scan(void)

{

USART1->DR=0Xa5;

while((USART1->SR&0X40)==0);

USART1->DR=0X20;

while((USART1->SR&0X40)==0);

}

需根据激光雷达通讯时序编写程序，下面代码为为起始应答后，数据应答处理程序如下：

if(response_mode)//在数据应答情况下的处理，

      case 1:if((res&0x01)!=1)//接收数据未能通过校验

                {

                            RX_BUF1[res_date_bit].packetstate=0;break;

                }

                    RX_BUF1[res_date_bit].angle=res>>1;

RX_BUF1[res_date_bit].packetstate++;break;

case2:RX_BUF1[res_date_bit].angle+=res<<7;

RX_BUF1[res_date_bit].packetstate++;break;             

      case 3:if(RX_BUF1[res_date_bit].quality>10)

                      RX_BUF1[res_date_bit].distance=res;     

                        else

                            RX_BUF1[res_date_bit].distance=0;

                            RX_BUF1[res_date_bit].packetstate++;break;}  

      case 4:if(RX_BUF1[res_date_bit].quality>10)

                 RX_BUF1[res_date_bit].distance+=(res<<8);    

               else

                    RX_BUF1[res_date_bit].distance=0;

                    RX_BUF1[res_date_bit].packetstate++;break;}

              if(RX_BUF1[res_date_bit].packetstate>=5)

                    {

                           RX_BUF1[res_date_bit].packetstate=0;

                           res_date_bit++;

                     }

             if(res_date_bit>=359)

             {

                    res_date_bit=0;

                    USART_RX_STA=2;

             }

}
{
      switch(RX_BUF1[res_date_bit].packetstate)
      {
      case 0: if((res&0x03)==3||(res&0x03)==0)//接收数据校验不通过
                {
                            RX_BUF1[res_date_bit].packetstate=0;break;  
                }
                    RX_BUF1[res_date_bit].quality=res>>2;
RX_BUF1[res_date_bit].packetstate++;break;
      case 1:if((res&0x01)!=1)//接收数据未能通过校验
                {
                            RX_BUF1[res_date_bit].packetstate=0;break;
                }
                    RX_BUF1[res_date_bit].angle=res>>1;
RX_BUF1[res_date_bit].packetstate++;break;
case2:RX_BUF1[res_date_bit].angle+=res<<7;
RX_BUF1[res_date_bit].packetstate++;break;             
      case 3:if(RX_BUF1[res_date_bit].quality>10)
                      RX_BUF1[res_date_bit].distance=res;     
                        else
                            RX_BUF1[res_date_bit].distance=0;
                            RX_BUF1[res_date_bit].packetstate++;break;}  
      case 4:if(RX_BUF1[res_date_bit].quality>10)
                 RX_BUF1[res_date_bit].distance+=(res<<8);    
               else
                    RX_BUF1[res_date_bit].distance=0;
                    RX_BUF1[res_date_bit].packetstate++;break;}
              if(RX_BUF1[res_date_bit].packetstate>=5)
                    {
                           RX_BUF1[res_date_bit].packetstate=0;

                           res_date_bit++;
                     }
             if(res_date_bit>=359)
             {
                    res_date_bit=0;
                    USART_RX_STA=2;
             }

}

当response_mode=1后，即在接收到正确的起始应答后，开始对数据进行接收，因为时序图有5个8位，即5个字节，所以我们需根据时序来处理数据，首先处理第一个8位，若第一位和第二位不等，即第一步校验通过，并开始接收quality，即第一个8位的高6位，并移到下一个8位，如果第一位和第二位相等校验即不通过，则对下一组接收到数据继续进行校验，直到通过。在第二个8位中，首先校验第一位C，若通过，开始接收angle的低7位，并移到第三个8位，若不通过，则对下一组接收到数据继续进行校验，直到通过。在第三个8位中，接收angle的高8位，并移到第四个8位。在第四个8位中，接收distance的第8位，并移到第5个8位。在第5个8位中接收distance的高8位，至此为止，这一组数据接收完成。当接收到360组数据后，开始对数据进行分析，其标志位为USART_RX_STA=2。