有偿帮忙解决HMC5883数据不准问题

谭春林

各位大神！
最近小弟在做一个测量空间方位角的设备，用的是HMC5883芯片，ADXL345
目前的状况：在校准之后，水平方位的方位角还算误差不大，基本误差在5度之内，但是设备产生俯仰角和横滚角时，方位角基本就不对了
我目前用的算法就是网上可以搜索到的算法，但是不知道是哪里出问题了！



下面是我的方位角处理代码：

1. void Read_HMC5883_data()
   
2. {
   
3.                uint8_t i;
   
4.                short X_sum[10],Y_sum[10],Z_sum[10];
   
5.                for(i=0;i<10;i++)
   
6.                {
   
7.                         Multiple_read_HMC5883();
   
8.                         X=((BUF[0] << 8) | BUF[1]); //Combine MSB and LSB of X Data output register
   
9.                         Z=((BUF[2] << 8) | BUF[3]); //Combine MSB and LSB of Z Data output register
   
10.                         Y=((BUF[4] << 8) | BUF[5]); //Combine MSB and LSB of Y Data output register
    
11.                          X_sum[i]=X;
    
12.                          Y_sum[i]=Y;
    
13.                          Z_sum[i]=Z;
    
14.                  }
    
15.                 X=Val_Filter(X_sum,10,2)-Xoffset;
    
16.                 Y=Val_Filter(Y_sum,10,2)-Yoffset;
    
17.                 Z=Val_Filter(Z_sum,10,2)-Zoffset;
    
18.                  
    
19.                cos_x=cos((double)((angx-900)/10.0*3.14159265/180.0));
    
20.                cos_y=cos((double)(-angy/10.0*3.14159265/180.0));
    
21.                sin_x=sin((double)((angx-900)/10.0*3.14159265/180.0));
    
22.                sin_y=sin((double)(-angy/10.0*3.14159265/180.0));
    
23.           
    
24.                 atan2_x=X*cos_x+Y*sin_y*sin_x-Z*cos_y*sin_x;
    
25.                atan2_y=Y*cos_y+Z*sin_y;
    
26.                  
    
27.                  if(atan2_x<0)
    
28.                  Angle_X=180-(atan2(((double)atan2_y*K),(double)atan2_x)* (180.0 / 3.14159265));
    
29.                  
    
30.                  if((atan2_x>0)&&(atan2_y<0))          
    
31.                  Angle_X=-(atan2(((double)atan2_y*K),(double)atan2_x)* (180.0 / 3.14159265));
    
32.                  
    
33.                  if((atan2_x>0)&&(atan2_y>0))          
    
34.                  Angle_X=360-(atan2(((double)atan2_y*K),(double)atan2_x)* (180.0 / 3.14159265));
    
35.                  
    
36.                  if((atan2_x==0)&&(atan2_y<0))          
    
37.                  Angle_X=90;
    
38.                  
    
39.                  if((atan2_x==0)&&(atan2_y>0))          
    
40.                  Angle_X=270;
    
41. }

复制代码

对这块熟悉的可以私信我，有偿解决，非常感谢！