|
各位大神!
最近小弟在做一个测量空间方位角的设备,用的是HMC5883芯片,ADXL345
目前的状况:在校准之后,水平方位的方位角还算误差不大,基本误差在5度之内,但是设备产生俯仰角和横滚角时,方位角基本就不对了
我目前用的算法就是网上可以搜索到的算法,但是不知道是哪里出问题了!
下面是我的方位角处理代码:
- void Read_HMC5883_data()
- {
- uint8_t i;
- short X_sum[10],Y_sum[10],Z_sum[10];
- for(i=0;i<10;i++)
- {
- Multiple_read_HMC5883();
- X=((BUF[0] << 8) | BUF[1]); //Combine MSB and LSB of X Data output register
- Z=((BUF[2] << 8) | BUF[3]); //Combine MSB and LSB of Z Data output register
- Y=((BUF[4] << 8) | BUF[5]); //Combine MSB and LSB of Y Data output register
- X_sum[i]=X;
- Y_sum[i]=Y;
- Z_sum[i]=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;
- }
复制代码
对这块熟悉的可以私信我,有偿解决,非常感谢! |
本帖子中包含更多资源
您需要 登录 才可以下载或查看,没有帐号?注册
x
阿莫论坛20周年了!感谢大家的支持与爱护!!
如果天空是黑暗的,那就摸黑生存;
如果发出声音是危险的,那就保持沉默;
如果自觉无力发光,那就蜷伏于牆角。
但是,不要习惯了黑暗就为黑暗辩护;
也不要为自己的苟且而得意;
不要嘲讽那些比自己更勇敢的人。
我们可以卑微如尘土,但不可扭曲如蛆虫。
|