维特WT931 姿态角度传感器（陀螺仪）使用记录

Posted 2021-10-06 Quark Star

传感器型号：WT931

使用目的：获取载体的俯仰(Pitch)、滚转(Roll)和偏航(Yaw)等姿态信息。

使用说明书下载：维特智能

使用大致步骤（主要根据说明书来）：①使用USB-TTL 串口模块连接传感器和上位机软件，在交互界面上设置需要读取的传感器采集数据，这里要获取的是姿态角的变化量和变化率（不需要的就不勾选，这样更不会浪费计算时间）；②进行磁场校准，磁场校准要远离强磁源（手动校准效果较差）；③选好串口通信波特率，调用STM32f10x系列单片机的串口通信库函数。④特别注意USAR T初始化函数的设置：匹配好波特率，引脚号、通道号、USART时钟和GPIO时钟。

RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);	//使能USART时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);   //使能GPIO时钟

注意：九轴模式可以获得精度更高的姿态，使用九轴模式之前一定要校准，不然Yaw的输出会有很大的误差。

使用说明书中，7.2上位机至模块的这部分通信协议一般是不需要在写入嵌入式芯片中的，因为这部分是用来做传感器使用前设置的。这在上位机软件上就可以实现，除非要实现多种模式在线切换。7.1模块至上位机的才需要写进主控芯片中。

 需要获取的是载体的姿态角和角速度，可以根据以下协议来编写串口中断服务程序。

     

注：

1. 姿态角结算时所使用的坐标系为东北天坐标系，正方向放置模块,如下图所示向左为X轴，向前为Y轴，向上为Z轴。欧拉角表示姿态时的坐标系旋转顺序定义为为 Z-Y-X,即先绕Z轴转，再绕Y轴转,再绕X轴转。

2. 滚转角的范围虽然是±180 度，但实际上由于坐标旋转顺序是 Z-Y-X，在表示姿态的时候，俯仰角(Y 轴)的范围只有±90 度，超过 90 度后会变换到小于 90 度，同时 让X轴的角度大于180度。欧拉角及姿态表示的相关信息详细原理见 对姿态角/欧拉角的理解_Quark Star的博客-CSDN博客 。

 以下是串口中断服务程序：

u16 USART_RX_STA=0;       //接收状态标记	 
u8  USART_RX_BUF[USART_REC_LEN];     //接收缓冲，最大USART_REC_LEN个字节
int n=0;
void USART3_IRQHandler(void)                	//串口3中断服务程序
	{

 	    u8 Res;

#if SYSTEM_SUPPORT_OS 		//如果SYSTEM_SUPPORT_OS为真，则需要支持OS
	OSIntEnter();    
#endif
	if(USART_GetITStatus(USART3, USART_IT_RXNE) != RESET)  //串口接收中断触发
    {
	  	Res =USART_ReceiveData(USART3);	                   //取当前接收的一个字节
		  if(n<=10)                                        //还没接收到完整的一帧数据
                {
                   if(Res==0x55&&n==0) USART_RX_STA|=0x8000;		//检测帧头字节，并在接收状态寄存器上（最高位置1）做下标记	 								                                      
                   if(USART_RX_STA&0x8000)
                   {  
					  if(n==1&&Res==0x53) USART_RX_STA|=0x4000;     //检测是否为角度帧，并在接收状态寄存器上（高第二位置1）做下标记
					  if(n==1&&Res==0x52) USART_RX_STA|=0x2000;		//检测是否为角速度帧，并在接收状态寄存器上（高第三位置1）做下标记							 
                      USART_RX_BUF[n]=Res ;                         //缓存当前接收的这个字节
		                  USART_RX_STA++ ;                          //接收状态寄存器记录此帧数据的接收进程
		                  n=USART_RX_STA&0x1FFF ;                   //转接进程记录
                    }										
					if(n==2) 
					{
					    if(((USART_RX_STA&0xf000)!=0xc000)&&((USART_RX_STA&0xf000)!=0xa000))  //检测是否成功接收帧前两字节
						{
                          USART_RX_STA=0 ;                           //接收状态寄存器清零
		                  n=USART_RX_STA&0x1FFF;                     //重新开始接收
                        } 
					}
                 }          
                   if(n==11)                                         //成功接收到一帧数据
                   {       
                      if((USART_RX_STA&0xf000)==0xc000)              //检测是否为角度帧
                      {
                         RollL=USART_RX_BUF[2];
                         RollH=USART_RX_BUF[3];
                         PitchL=USART_RX_BUF[4];
                         PitchH=USART_RX_BUF[5];
                         YawL=USART_RX_BUF[6];
                         YawH=USART_RX_BUF[7];
                         TL=USART_RX_BUF[8];
                         TH=USART_RX_BUF[9];
                         SUM=USART_RX_BUF[10];
                         if(SUM==(0x55+0x53+RollH+RollL+PitchH+PitchL+YawH+YawL+TH+TL)%0x100) //和校验，注意这里去除了高八位上的和
                         { 
                            Roll=(short)((RollH<<8)|RollL)/32768.0*180;
                            Pitch=(short)((PitchH<<8)|PitchL)/32768.0*180;
                            Yaw=(short)((YawH<<8)|YawL)/32768.0 *180;
		                        n=0;	              //数据处理完成
													  B=USART_RX_STA;   
                            USART_RX_STA=0;			  //清零，待重新接收										 
                          }
                         else
                           {
                             USART_RX_STA=0 ;
		                         n=0; 
                            }                  
                       }
                       if((USART_RX_STA&0xf000)==0xa000)              //检测是否为角速度帧
                      {
                         wxL=USART_RX_BUF[2]; 
                         wxH=USART_RX_BUF[3];
                         wyL=USART_RX_BUF[4];
                         wyH=USART_RX_BUF[5];
                         wzL=USART_RX_BUF[6];
                         wzH=USART_RX_BUF[7];
                         TL=USART_RX_BUF[8];
                         TH=USART_RX_BUF[9];
                         SUM=USART_RX_BUF[10];
                         if(SUM==(0x55+0x52+wxH+wxL+wyH+wyL+wzH+wzL+TH+TL)%0x100) //和校验，注意这里去除了高八位上的和
                         {
                            wx=(short)((wxH<<8)|wxL)/32768.0*2000;
                            wy=(short)((wyH<<8)|wyL)/32768.0*2000;
                            wz=(short)((wzH<<8)|wzL)/32768.0*2000;                           
		                        n=0;                 //数据处理完成
													  A=USART_RX_STA;
													  USART_RX_STA=0;  	//清零，待重新接收									 
                          }
                         else         //接收错误
                           {
                             USART_RX_STA=0 ;
		                         n=0; 
                            }         //清零，待重新接收         
                       } 
                    }	
                    if(n>=11)         //数据处理结束
                     {
                       USART_RX_STA=0 ;
		                   n=0; 
                    }                 //清零，待重新接收
              if(A==0xa00b&&B==0xc00b) USART_ITConfig(USART3, USART_IT_RXNE, DISABLE);//本次串口中断服务顺利完成 			
	 } 
#if SYSTEM_SUPPORT_OS 	//如果为SYSTEM_SUPPORT_OS真，则需要支持OS
	OSIntExit();  											 
#endif
}