Diff: main.cpp

Revision:

4:45dc5590abc0

Parent:

3:5358a691a100

Child:

5:182f6356bce1

--- a/main.cpp	Sat May 30 18:08:34 2015 +0000
+++ b/main.cpp	Thu Jun 11 15:43:07 2015 +0000
@@ -18,7 +18,8 @@
 /********** private typedef   **********/
 
 /********** private variables **********/
-I2C         i2c(PB_9, PB_8);      // I2Cポート
+DigitalOut myled(LED1);                 // デバッグ用LEDのためのデジタル出力
+I2C         i2c(PB_9, PB_8);            // I2Cポート
 MPU6050     mpu(&i2c);                  // 加速度・角速度センサ
 HMC5883L    hmc(&i2c);                  // 地磁気センサ
 LPS25H      lps(&i2c);                  // 気圧センサ
@@ -26,18 +27,24 @@
 Serial      pc(SERIAL_TX, SERIAL_RX);   // PC通信用シリアルポート
 GMS6_CR6    gms(&gps, &pc);             // GPS
 Ticker      INT_timer;                      // 割り込みタイマー
-//Log         logger(PA_9, PA_10, PB_5, PB_4, PB_3, PB_10, "sd");    // ロガー（microSD、XBee）
+Log         logger(PA_9, PA_10, PC_1, PB_5, PB_4, PB_3, PB_10);    // ロガー（microSD、XBee）
+DigitalIn   cts(PC_1);
 
 const float dt = 0.1f;           // 割り込み周期（s）
 
 int lps_cnt = 0;                    // 気圧センサ読み取りカウント
 uint8_t INT_flag = TRUE;            // 割り込み可否フラグ
+Vector raw_acc(3);                       // 加速度(m/s^2)  生
+Vector raw_gyro(3);                      // 角速度(deg/s)  生
+Vector raw_geomag(3);                    // 地磁気(?)  生
+float raw_press;                        // 気圧(hPa)  生
 Vector acc(3);                       // 加速度(m/s^2)
 Vector gyro(3);                      // 角速度(deg/s)
 Vector geomag(3);                    // 地磁気(?)
 float press;                        // 気圧(hPa)
 
-Vector g(3);                        // 重力ベクトル
+Vector raw_g(3);                        // 重力ベクトル  生
+Vector g(3);                        // 重力ベクトル  生
 //Vector n(3);                        // 地磁気ベクトル
 
 /* ----- Kalman Filter ----- */
@@ -57,8 +64,8 @@
 char data[1024] = {};
 
 /********** private functions **********/
-void KalmanInit();
-void KalmanUpdate();
+void KalmanInit();                  // カルマンフィルタ初期化
+void KalmanUpdate();                // カルマンフィルタ更新
 void INT_func();                    // 割り込み用関数
 void toString(Matrix& m);
 void toString(Vector& v);
@@ -72,57 +79,44 @@
     if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!");        // mpu6050初期化
     if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!");       // hmc5883l初期化
     
-    //char* title = "log data\r\n";                                       // ログのタイトル行
-    //if(!logger.initialize_sdlog(title)) return 0;                       // ログファイル初期設定
+    char* title = "log data\r\n";                                       // ログのタイトル行
+    if(!logger.initialize_sdlog(title)) return 0;                       // ログファイル初期設定
     
     KalmanInit();
     
     INT_timer.attach(&INT_func, dt);  // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
     
     //重力ベクトルの初期化
-    g.SetComp(1, 0.0f);
-    g.SetComp(2, 0.0f);
-    g.SetComp(3, 1.0f);
+    raw_g.SetComp(1, 0.0f);
+    raw_g.SetComp(2, 0.0f);
+    raw_g.SetComp(3, 1.0f);
     
     /* ---------- ↓↓↓ ここからメインループ ↓↓↓ ---------- */
     while(1) {
-        
+        timer.stop();
+        timer.reset();
+        timer.start();
         // 0.1秒おきにセンサーの出力をpcへ出力
-        wait(0.1f);
+        myled = 1; // LED is ON
+        wait(0.05f); // 50 ms
+        myled = 0; // LED is OFF
         
         INT_flag = FALSE;           // 割り込みによる変数書き換えを阻止
         
-        // センサ類の全出力値をPCに送信
-        /*
-        pc.printf("%.3f,", acc.GetComp(1));
-        pc.printf("%.3f,", acc.GetComp(2));
-        pc.printf("%.3f,", acc.GetComp(3));
-        pc.printf("%.3f,", gyro.GetComp(1));
-        pc.printf("%.3f,", gyro.GetComp(2));
-        pc.printf("%.3f,", gyro.GetComp(3));
-        pc.printf("%.3f,", geomag.GetComp(1));
-        pc.printf("%.3f,", geomag.GetComp(2));
-        pc.printf("%.3f\r\n", geomag.GetComp(3));
-        */
-        //pc.printf("%.3f\r\n", press);
-        
-        
-        pc.printf("%.3f,", geomag.GetComp(1));
-        pc.printf("%.3f,", geomag.GetComp(2));
-        pc.printf("%.3f,", geomag.GetComp(3));
-        pc.printf("%.3f,", post_x.GetComp(1));
-        pc.printf("%.3f,", post_x.GetComp(2));
-        pc.printf("%.3f\r\n", post_x.GetComp(3));
-        
-        /*
-        sprintf(data, "%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", 
+        sprintf(data, "%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", 
                 acc.GetComp(1), acc.GetComp(2), acc.GetComp(3), 
                 gyro.GetComp(1), gyro.GetComp(2), gyro.GetComp(3), 
-                geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3), press);
+                geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3), 
+                press, gms.longitude, gms.latitude);
         logger.puts(data);
-        */
+        
         INT_flag = TRUE;            // 割り込み許可
         
+        pc.printf("time: %.3f[s]\r\n", (float)timer.read_ms()/1000.0f);
+        
+        // ループはきっかり1秒ごと
+        while(timer.read_ms() < 1000);
+        
     }
     
     /* ---------- ↑↑↑ ここまでメインループ ↑↑↑ ---------- */
@@ -138,9 +132,9 @@
     
     // 状態方程式のヤコビアンの初期値を代入（時間変化あり）
     float f[36] = {
-        1.0f, gyro.GetComp(3)*dt, -gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f, 
-        -gyro.GetComp(3)*dt, 1.0f, gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f, 
-        gyro.GetComp(2)*dt, -gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f, 
+        1.0f, raw_gyro.GetComp(3)*dt, -raw_gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f, 
+        -raw_gyro.GetComp(3)*dt, 1.0f, raw_gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f, 
+        raw_gyro.GetComp(2)*dt, -raw_gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f, 
         0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 
         0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,  
         0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
@@ -161,9 +155,9 @@
 void KalmanUpdate() {
     // ヤコビアンの更新
     float f[36] = {
-        1.0f, gyro.GetComp(3)*dt, -gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f, 
-        -gyro.GetComp(3)*dt, 1.0f, gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f, 
-        gyro.GetComp(2)*dt, -gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f, 
+        1.0f, raw_gyro.GetComp(3)*dt, -raw_gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f, 
+        -raw_gyro.GetComp(3)*dt, 1.0f, raw_gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f, 
+        raw_gyro.GetComp(2)*dt, -raw_gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f, 
         0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 
         0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,  
         0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
@@ -186,42 +180,47 @@
     K = pri_P * H.Transpose() * inv;    
     
     // 事後推定値の更新
-    post_x = pri_x + K * (geomag - H * pri_x);
+    post_x = pri_x + K * (raw_geomag - H * pri_x);
     // 事後誤差分散行列の更新
     post_P = (I - K * H) * pri_P;
 }
 
 void INT_func() {
-    if(INT_flag != FALSE) {
+    // センサーの値を更新
+    mpu.read();
+    hmc.read();
+    
+    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);
+    }
         
-        timer.reset();
-        timer.start();
-        // センサーの値を更新
-        mpu.read();
-        hmc.read();
-        
-        for(int i=0; i<3; i++) {
-            acc.SetComp(i+1, (float)mpu.data.value.acc[i] * ACC_LSB_TO_G);
-            gyro.SetComp(i+1, (float)mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD);
-            geomag.SetComp(i+1, (float)hmc.data.value[i] * MAG_LSB_TO_GAUSS);
-        }
+    Vector delta_g = Cross(raw_gyro, raw_g);                            // Δg = ω × g
+    raw_g = 0.9f * (raw_g - delta_g * dt) + 0.1f * raw_acc.Normalize();   // 相補フィルタ
+    raw_g = raw_g.Normalize();
+       
+    KalmanUpdate();
         
-        Vector delta_g = Cross(gyro, g);                            // Δg = ω × g
-        g = 0.9f * (g - delta_g * dt) + 0.1f * acc.Normalize();   // 相補フィルタ
-        g = g.Normalize();
-        
-        
-        KalmanUpdate();
-        
+    // LPS25Hによる気圧の取得は10Hz
+    lps_cnt = (lps_cnt+1)%10;
+    if(lps_cnt == 0) {
+        raw_press = (float)lps.pressure() * PRES_LSB_TO_HPA;
+    }
+    //pc.printf("%d(us)\r\n", timer.read_us());
+    
+    //pc.printf("PC_1 = %d\r\n", (int)cts);
+    //pc.printf("test\r\n");
+    
+    if(INT_flag != FALSE) {
+        g = raw_g;
+        for(int i=0; i<3; i++) {
+            geomag.SetComp(i+1, post_x.GetComp(i+1));
+        }
+        acc = raw_acc;
+        gyro = raw_gyro;
+        press = raw_press;
         
-        // LPS25Hによる気圧の取得は10Hz
-        lps_cnt = (lps_cnt+1)%10;
-        if(lps_cnt == 0) {
-            press = (float)lps.pressure() * PRES_LSB_TO_HPA;
-        }
-        
-        timer.stop();
-        //pc.printf("%d(us)\r\n", timer.read_us());
     }
 }