Diff: main.cpp

Revision:

1:342818b81499

Parent:

0:704adb172e40

Child:

2:b257a71f0655

--- a/main.cpp	Fri Aug 10 18:18:00 2018 +0000
+++ b/main.cpp	Sun Aug 12 06:42:02 2018 +0000
@@ -294,15 +294,44 @@
 
 //方角だす
 void compass(){
+        double maveX = 0, maveY = 0, maveZ = 0;
+        int16_t ax, ay, az;
+        float accX, accY, accZ, acc;
         int16_t mx, my, mz;
         float magX, magY, magZ, mag;
-        int theta;
+        double rad;
+        double degree;
+        double roll;
+        double pitch;
+        //補正する為に取得
+        accX = ax * accRange / 32768.0;//[G]に変換
+        accY = ay * accRange / 32768.0;//[G]に変換
+        accZ = az * accRange / 32768.0;//[G]に変換
+        roll = atan2(accY, accZ);
+        pitch = atan2(-accX, sin(accY) + cos(accZ));
         //磁気の値を取得し、方位判定
-        Mag_Read(&mx, &my, &mz);
-        magX = (mx + 340.0f) / 32768.0f * 4800.0f;//[uT]に変換
-        magY = (my - 234.0f) / 32768.0f * 4800.0f;//[uT]に変換
+        for(int i = 0;i < 150;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]に変換
         magZ = mz / 32768.0f * 4800.0f;//[uT]に変換
-        theta = (int)(180.0 * atan2(magY, magX) / 3.14) + 180;
-        // 角度の表示
-        pc.printf("%d\n\r", theta);
+        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);
+        /*
+        if(degree < 0){
+            degree += 360;    
+        }
+        */
+        pc.printf("%d\n\r",(int)degree);
 }
\ No newline at end of file