Diff: main.cpp

Revision:

43:3a37e39b234c

Parent:

42:7428acb9b14d

--- a/main.cpp	Sat Dec 26 11:07:09 2015 +0000
+++ b/main.cpp	Sat Dec 26 11:44:09 2015 +0000
@@ -1,6 +1,6 @@
 #include "mbed.h"
 #include "MPU6050.h"
-//#include "HMC5883L.h"
+#include "HMC5883L.h"
 #include "LPS25H.h"
 #include "GMS6_CR6.h"
 #include "Vector.h"
@@ -19,10 +19,11 @@
 /****************************** private typedef   ******************************/
 /****************************** private variables ******************************/
 DigitalOut      myled(LED1);                        // デバッグ用LEDのためのデジタル出力
-I2C             i2c(p28,p27);                    // I2Cポート
-MPU6050         mpu(&i2c);                          // 加速度・角速度センサ
-//HMC5883L        hmc(&i2c);                          // 地磁気センサ
-LPS25H          lps(&i2c);                          // 気圧センサ
+I2C             i2c_mpu(p28,p27);                   // i2c_mpuポート
+I2C             i2c_hmc(p9,p10);
+MPU6050         mpu(&i2c_mpu);                          // 加速度・角速度センサ
+HMC5883L        hmc(&i2c_hmc);                          // 地磁気センサ
+//LPS25H          lps(&i2c_mpu);                          // 気圧センサ
 Serial          pc(USBTX,USBRX);           // PC通信用シリアルポート
 FILE *          fp;                                 // ログファイルのポインタ
 ConfigFile      cfg;                                // ConfigFile
@@ -146,14 +147,18 @@
 /******************************   main function   ******************************/
 
 int main()
-
 {
+        //重力ベクトルの初期化
+    raw_g.SetComp(1, 0.0f);
+    raw_g.SetComp(2, 0.0f);
+    raw_g.SetComp(3, 1.0f);
     //↓mpuのスリープモードを解除するコードを含んだ関数（SensorsInit()）が宣言だけされて実装されてなかったので追加(w)
     SensorsInit();
-    //↓i2c.startたしたけど変化なし
-    i2c.start();
-   // pc.printf("HELLO");
-    i2c.frequency(400000);                  // I2Cの通信速度を400kHzに設定
+    //↓i2c_mpu.startたしたけど変化なし
+    i2c_mpu.start();
+    i2c_hmc.start();
+    i2c_mpu.frequency(400000);  
+    i2c_hmc.frequency(400000);                // i2c_hmcの通信速度を400kHzに設定
     //カルマンフィルタ初期化
     KalmanInit();
 
@@ -161,8 +166,6 @@
 
     /* ------------------------------ ↓↓↓ ここからメインループ ↓↓↓ ------------------------------ */
     while(1) {
-        //pc.printf("roll:%7f,pitch:%7f,yaw:%7f\n\r",roll,pitch,yaw);
-        // pc.printf("HELLO");
         timer.stop();
         timer.reset();
         timer.start();
@@ -189,13 +192,7 @@
 {
 
     if(!mpu.init()) error("mpu6050 Initialize Error !!");        // mpu6050初期化
-    //if(!hmc.init()) error("hmc5883l Initialize Error !!");       // hmc5883l初期化
-
-    //重力ベクトルの初期化
-    raw_g.SetComp(1, 0.0f);
-    raw_g.SetComp(2, 0.0f);
-    raw_g.SetComp(3, 1.0f);
-    //toString(raw_g);
+    if(!hmc.init()) error("hmc5883l Initialize Error !!");       // hmc5883l初期化
 
     // 機体に固定されたベクトルの初期化
     b_f.SetComp(1, 0.0f);
@@ -208,7 +205,7 @@
 
     // 目標座標をベクトルに代入
     // target_p.SetComp(1, target_x * DEG_TO_RAD);
-    // target_p.SetComp(2, target_y * DEG_TO_RAD);
+     //target_p.SetComp(2, target_y * DEG_TO_RAD);
 }
 
 void KalmanInit()
@@ -270,7 +267,7 @@
         F2.SetComps(f2);
 
         // 事前推定値の更新
-        //pri_x2 = F2 * post_x2;
+        pri_x2 = F2 * post_x2;
 
         float pri_x2_vals[5] = {
             post_x2.GetComp(1) + post_x2.GetComp(2) * (raw_gyro.GetComp(3) - post_x1.GetComp(7)) * dt - post_x2.GetComp(3) * (raw_gyro.GetComp(2) - post_x2.GetComp(5)) * dt,
@@ -358,8 +355,8 @@
 {
     // センサーの値を更新
     mpu.read();
-    // hmc.read();
-    /*//↓i2cの通信に成功しているかどうかを確認（w）
+    hmc.read();
+    /*//↓i2c_mpuの通信に成功しているかどうかを確認（w）
      //===========================================================
     if(mpu.checker_get() == 0){
      pc.printf("SUCCESS!!\n\r");
@@ -372,7 +369,7 @@
     for(int i=0; i<3; i++) {
         raw_acc.SetComp(i+1, (float)mpu.data.value.acc[i] * ACC_LSB_TO_G);
         raw_gyro.SetComp(i+1, (float)mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD);
-        //   raw_geomag.SetComp(i+1, (float)hmc.data.value[i] * MAG_LSB_TO_GAUSS);
+         raw_geomag.SetComp(i+1, (float)hmc.data.value[i] * MAG_LSB_TO_GAUSS);
        //pc.printf("raw_acc/%d:%10f    raw_gyro/%d:%10f\n\r",i,raw_acc.GetComp(i), i,raw_gyro.GetComp(i));
     }
         raw_g.SetComp(1, 0.0f);
@@ -390,8 +387,7 @@
          pc.printf("%f",raw_g.GetComp(i));
      }
      */
-    //raw_g = raw_g.Normalize();
-    // toString(raw_g);
+    raw_g = raw_g.Normalize();
 //=========================================
 
     KalmanUpdate();
@@ -405,7 +401,6 @@
     if(1) {
 
         g = raw_g;
-        // toString(g);
         for(int i=0; i<3; i++) {
             geomag.SetComp(i+1, post_x1.GetComp(i+1));
         }
@@ -427,7 +422,6 @@
     // 参照座標系の基底を求める
     r_u = g;
     r_l = Cross(r_u, r_f);
-//toString(r_f);
     r_f = geomag.GetPerpCompTo(g).Normalize();
 
     // 回転行列を経由してオイラー角を求める
@@ -444,7 +438,6 @@
     } else {
         roll = -acos(r_cos) * RAD_TO_DEG;
     }
-//   toString(g.GetPerpCompTo(b_1).Normalize());
     p_cos = g.GetPerpCompTo(b_l).Normalize() * b_u;
     p_sin = Cross(g.GetPerpCompTo(b_l).Normalize(), b_u) * b_l;