|
当沿着X轴倾斜的时候,罗盘角度变化很小(不补偿的时候变化大),但是沿着Y轴倾斜的时候,水平补偿似乎一点没有作用。
主程序如下:
Init_HMC5883();
Multiple_read_HMC5883();
x=BUF[0] << 8 | BUF[1];
z=BUF[2] << 8 | BUF[3];
y=BUF[4] << 8 | BUF[5];
angle= atan2((double)y,(double)x) * (180 / 3.14159265) + 180;
Init_MMA8452(); //初始化MMA8452
Multiple_read_MMA8452(); //连续读出数据,存储在BUF中
dis_data=(BUF[0]<<8)|BUF[1]; //合成数据
dis_data>>=4;
xx=(float)dis_data;
dis_data=(BUF[2]<<8)|BUF[3]; //合成数据
dis_data>>=4;
yy=(float)dis_data;
dis_data=(BUF[4]<<8)|BUF[5]; //合成数据
dis_data>>=4;
zz=(float)dis_data;
ztemp=atan(sqrt(xx*xx+yy*yy)/zz);
ztemp=ztemp*180/3.14;
ytemp=atan(yy/sqrt(xx*xx+zz*zz));
ytemp=ytemp*180/3.14;
xtemp=atan(xx/sqrt(yy*yy+zz*zz));
xtemp=xtemp*180/3.14;
ctpx=cos((float)(xtemp*3.14/180));
stpx=sin((float)(xtemp*3.14/180));
ctpy=cos((float)(ytemp*3.14/180));
stpy=sin((float)(ytemp*3.14/180));
xout=((float)x*ctpx+(float)y*stpy*stpx-(float)z*ctpy*stpx);
yout=((float)y*ctpy+(float)z*stpy);
anglebc= atan2(yout,xout) * (180 / 3.14159265) + 180;
Delayms(100);
因为程序写的很乱,所以就不把全部程序发上来了,需要的和我说下,我发给你。
希望高手帮我解决下这个问题,本人数学比较差 公式都是网上找的。。 |
阿莫论坛20周年了!感谢大家的支持与爱护!!
知道什么是神吗?其实神本来也是人,只不过神做了人做不到的事情 所以才成了神。 (头文字D, 杜汶泽)
|