Diff: main.cpp

Revision:

4:1d0dbfceeaed

Parent:

3:7644f7b86ae0

--- a/main.cpp	Sun Aug 12 08:44:07 2018 +0000
+++ b/main.cpp	Tue Aug 14 03:52:29 2018 +0000
@@ -60,24 +60,24 @@
         pitch = atan2(-accX, sin(accY) + cos(accZ));
         i2cWrite(MPU9250_ADDRESS, MAG_OPN, 0x02); //磁気センサの起動
         i2cWrite(AK8963_ADDRESS, MAG_CONFIG, MAG_100HZ);//磁気センサの測定レンジの設定
-        for(int i = 0;i < 150;i++){
+        for(int i = 0;i < 50;i++){
             Mag_Read(&mx, &my, &mz);
             maveX += mx;
             maveY += my;
             maveZ += mz;
             wait_ms(10);
         }          
-        maveX /= 150;
-        maveY /= 150;
-        maveZ /= 150;
-        magX = (maveX - 168.75) / 32768.0f * 4800.0f;//[uT]に変換
-        magY = (maveY - 18.75) / 32768.0f * 4800.0f;//[uT]に変換
+        maveX /= 50;
+        maveY /= 50;
+        maveZ /= 50;
+        magX = (maveX + 11.44) / 32768.0f * 4800.0f;//[uT]に変換
+        magY = (maveY + 12.1) / 32768.0f * 4800.0f;//[uT]に変換
         magZ = mz / 32768.0f * 4800.0f;//[uT]に変換
         pc.printf("%f,%f\n\r", magX, magY);
         //ID = IDcheck();
         //pc.printf("%f\n\r", ID);
         rad = atan2(magZ * sin(roll) - magY * cos(roll), magX * cos(pitch) + magY * sin(pitch) * sin(roll) + magZ * sin(pitch) * cos(roll));
-        degree = -((int)(rad * 180.0 / 3.141592 + 270.0 - 7.5) % 360 - 360.0);
+        degree = -((int)(rad * 180.0 / 3.141592 + 270.0 - 8.34) % 360 - 360.0);
         /*
         if(degree < 0){
             degree += 360;    
@@ -104,9 +104,9 @@
     i2cRead(AK8963_ADDRESS, 0x03, 7, magneticdata);
   }
   else pc.printf("ERROR!!\n");
-  *mx = (magneticdata[0] << 8) | magneticdata[1];
-  *my = (magneticdata[2] << 8) | magneticdata[3];
-  *mz = (magneticdata[4] << 8) | magneticdata[5];
+  *mx = ((int16_t)magneticdata[3] << 8) | (int16_t)magneticdata[2];
+  *my = ((int16_t)magneticdata[1] << 8) | (int16_t)magneticdata[0];
+  *mz = -((int16_t)magneticdata[5] << 8) | (int16_t)magneticdata[4];
 }
 
 //mpu9250からデータを取得(bytesに受け取るデータのバイト数、dataに実際のデータを挿入していく)