Diff: main.cpp

Revision:

5:7eb3b7e56eb1

Parent:

4:1d0dbfceeaed

Child:

6:eeef849dd7a1

--- a/main.cpp	Tue Aug 14 03:52:29 2018 +0000
+++ b/main.cpp	Tue Aug 14 14:07:45 2018 +0000
@@ -1,4 +1,4 @@
-/*MPU9250_PROGRAM ver? PROGRAMED BY RYOTARO FUNAI 2018/08/12*/
+/*MPU9250_PROGRAM ver4.1 PROGRAMED BY RYOTARO FUNAI 2018/08/14*/
 #include <mbed.h>
 #include <math.h>
 
@@ -32,34 +32,41 @@
 void i2cRead(uint8_t addr,uint8_t reg, uint8_t bytes,uint8_t* data);
 void i2cWrite(uint8_t addr,uint8_t reg, uint8_t data);
 uint8_t IDcheck();
+int EarthMag();
 
 uint8_t accgyrodata[14];
 uint8_t magneticdata[7];
 uint8_t ST2_Bit;  //磁気センサのステータスを入れておく
 
-int16_t mx, my, mz;
-double magX, magY, magZ, mag;
-int16_t ax, ay, az;
-float accX, accY, accZ, acc;
-double rad;
-double degree;
-float ID;
-double roll;
-double pitch;
+int main() {
+    EarthMag(); //加速度センサの値は引数として渡したらいいかも
+}
 
-int main() {
-    i2cWrite(MPU9250_ADDRESS, PWR, 0x00);  //スリープモードの解除
+int EarthMag(){
+    //方位角計測用各種変数群
+    int16_t mx, my, mz;
+    double magX, magY, magZ, mag;
+    int16_t ax, ay, az;
+    float accX, accY, accZ, acc;
+    double rad;     //単位はラジアン
+    double degree;  //単位は度数
+    double roll;    //ロール軸の角度
+    double pitch;   //ピッチ軸の角度
     double maveX = 0, maveY = 0, maveZ = 0;
+    
+    //地磁気センサの設定
+    i2cWrite(MPU9250_ADDRESS, PWR, 0x00);  //スリープモードの解除
+    i2cWrite(MPU9250_ADDRESS, MAG_OPN, 0x02); //磁気センサの起動
+    i2cWrite(AK8963_ADDRESS, MAG_CONFIG, MAG_100HZ);//磁気センサの測定レンジの設定        
+    
     while(1) {
-        //加速度の値を取得し、落下判定
+        //z軸方向の誤差を取るためのロールとピッチ
         Ac_Read(&ax,&ay,&az);
         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));
-        i2cWrite(MPU9250_ADDRESS, MAG_OPN, 0x02); //磁気センサの起動
-        i2cWrite(AK8963_ADDRESS, MAG_CONFIG, MAG_100HZ);//磁気センサの測定レンジの設定
         for(int i = 0;i < 50;i++){
             Mag_Read(&mx, &my, &mz);
             maveX += mx;
@@ -74,15 +81,8 @@
         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 - 8.34) % 360 - 360.0);
-        /*
-        if(degree < 0){
-            degree += 360;    
-        }
-        */
         pc.printf("%d\n\r",(int)degree);
     }
 }