|
|
獲取原始數(shù)據(jù)，然后計(jì)算
附件是使用了OLED 0.96來做的指南針，雖然YZ軸沒啥用，但是還是做了處理；
和iphone對(duì)比，基本上做到了5度之內(nèi)的偏差；
Multiple_Read_QMC5883(BUF);
magx=BUF[1] << 8 | BUF[0]; //Combine MSB and LSB of X Data output register 最高有效位
magy=BUF[3] << 8 | BUF[2]; //Combine MSB and LSB of Y Data output register
magz=BUF[5] << 8 | BUF[4]; //Combine MSB and LSB of Z Data output register
if(magx>0x7fff)magx-=0xffff;
if(magy>0x7fff)magy-=0xffff;
if(magz>0x7fff)magz-=0xffff;
magx=magx-offsetX;magy=magy-offsetY;magz=magz-offsetZ;
Angle_XY= atan2((double)magy,(double)magx) * (180 / 3.14159265) + 180; //計(jì)算XY平面角度
Angle_XZ= atan2((double)magz,(double)magx) * (180 / 3.14159265) + 180; //計(jì)算XZ平面角度
Angle_YZ= atan2((double)magz,(double)magy) * (180 / 3.14159265) + 180; //計(jì)算YZ平面角
當(dāng)然這還是不夠的，因?yàn)榇艌?chǎng)便宜，或當(dāng)?shù)卮艌?chǎng)問題會(huì)造成，xyz擬合的時(shí)候出現(xiàn)橢圓，所以要修正；
Multiple_Read_QMC5883(BUF); //連續(xù)讀出數(shù)據(jù)，存儲(chǔ)在BUF中
X=BUF[1] << 8 | BUF[0]; //Combine MSB and LSB of X Data output register 最高有效位
Y=BUF[3] << 8 | BUF[2]; //Combine MSB and LSB of Y Data output register
Z=BUF[5] << 8 | BUF[4]; //Combine MSB and LSB of Z Data output register
if(X>0x7fff)X-=0xffff;
if(Y>0x7fff)Y-=0xffff;
if(Z>0x7fff)Z-=0xffff;
if (X > xMax)
xMax = X;
if (X < xMin )
xMin = X;
if(Y > yMax )
yMax = Y;
if(Y < yMin )
yMin = Y;
if(Z > zMax )
zMax = Z;
if(Z < zMin )
zMin = Z;
if(abs(xMax - xMin) > 0 )
offsetX = (xMax + xMin)/2;
if(abs(yMax - yMin) > 0 )
offsetY = (yMax + yMin)/2;
if(abs(zMax - zMin) > 0 )
offsetZ = (zMax +zMin)/2;
Keil代碼下載:
51單片機(jī)代碼.7z
(57.28 KB, 下載次數(shù): 199)
2023-3-17 18:00 上傳
點(diǎn)擊文件名下載附件
|
評(píng)分
-
查看全部評(píng)分
|
