有偿帮忙解决HMC5883数据不准问题
各位大神!最近小弟在做一个测量空间方位角的设备,用的是HMC5883芯片,ADXL345
目前的状况:在校准之后,水平方位的方位角还算误差不大,基本误差在5度之内,但是设备产生俯仰角和横滚角时,方位角基本就不对了
我目前用的算法就是网上可以搜索到的算法,但是不知道是哪里出问题了!
下面是我的方位角处理代码:
void Read_HMC5883_data()
{
uint8_t i;
short X_sum,Y_sum,Z_sum;
for(i=0;i<10;i++)
{
Multiple_read_HMC5883();
X=((BUF << 8) | BUF); //Combine MSB and LSB of X Data output register
Z=((BUF << 8) | BUF); //Combine MSB and LSB of Z Data output register
Y=((BUF << 8) | BUF); //Combine MSB and LSB of Y Data output register
X_sum=X;
Y_sum=Y;
Z_sum=Z;
}
X=Val_Filter(X_sum,10,2)-Xoffset;
Y=Val_Filter(Y_sum,10,2)-Yoffset;
Z=Val_Filter(Z_sum,10,2)-Zoffset;
cos_x=cos((double)((angx-900)/10.0*3.14159265/180.0));
cos_y=cos((double)(-angy/10.0*3.14159265/180.0));
sin_x=sin((double)((angx-900)/10.0*3.14159265/180.0));
sin_y=sin((double)(-angy/10.0*3.14159265/180.0));
atan2_x=X*cos_x+Y*sin_y*sin_x-Z*cos_y*sin_x;
atan2_y=Y*cos_y+Z*sin_y;
if(atan2_x<0)
Angle_X=180-(atan2(((double)atan2_y*K),(double)atan2_x)* (180.0 / 3.14159265));
if((atan2_x>0)&&(atan2_y<0))
Angle_X=-(atan2(((double)atan2_y*K),(double)atan2_x)* (180.0 / 3.14159265));
if((atan2_x>0)&&(atan2_y>0))
Angle_X=360-(atan2(((double)atan2_y*K),(double)atan2_x)* (180.0 / 3.14159265));
if((atan2_x==0)&&(atan2_y<0))
Angle_X=90;
if((atan2_x==0)&&(atan2_y>0))
Angle_X=270;
}
对这块熟悉的可以私信我,有偿解决,非常感谢!
页:
[1]