Diff: main.cpp

Revision:

1:6cd6d2760856

Parent:

0:bc6f14fc60c7

Child:

2:d2b60a1d0cd9

--- a/main.cpp	Fri May 15 17:24:32 2015 +0000
+++ b/main.cpp	Sun May 24 17:32:47 2015 +0000
@@ -2,6 +2,7 @@
 #include "MPU6050.h"
 #include "HMC5883L.h"
 #include "LPS25H.h"
+#include "GMS6_CR6.h"
 #include "ErrorLogger.h"
 #include "Vector.h"
 #include "myConstants.h"
@@ -10,25 +11,35 @@
 #define TRUE    1
 #define FALSE   0
 /********** private macro     **********/
+
 /********** private typedef   **********/
+
 /********** private variables **********/
-I2C         i2c(I2C_SDA, I2C_SCL);  // I2Cポート
-MPU6050     mpu(&i2c);              // 加速度・角速度センサ
-HMC5883L    hmc(&i2c);              // 地磁気センサ
-LPS25H      lps(&i2c);              // 気圧センサ
-Serial pc(SERIAL_TX, SERIAL_RX);    // PC通信用シリアルポート
-Ticker timer;                       // 割り込みタイマー
+I2C         i2c(I2C_SDA, I2C_SCL);      // I2Cポート
+MPU6050     mpu(&i2c);                  // 加速度・角速度センサ
+HMC5883L    hmc(&i2c);                  // 地磁気センサ
+LPS25H      lps(&i2c);                  // 気圧センサ
+Serial      gps(D8, D2);             // GPS通信用シリアルポート
+Serial      pc(SERIAL_TX, SERIAL_RX);   // PC通信用シリアルポート
+GMS6_CR6    gms(&gps, &pc);             // GPS
+Ticker      timer;                      // 割り込みタイマー
 
-const float freq = 0.01f;           // 割り込み周期（s）
+const float Freq = 0.01f;           // 割り込み周期（s）
 
 int lps_cnt = 0;                    // 気圧センサ読み取りカウント
-uint8_t INT_flag = TRUE;            // 割り込み可否フラグ
-float acc[3];                       // 加速度(m/s^2)
-float gyro[3];                      // 角速度(deg/s)
-float geomag[3];                    // 地磁気(?)
+uint8_t INT_flag = FALSE;            // 割り込み可否フラグ
+Vector acc(3);                       // 加速度(m/s^2)
+Vector gyro(3);                      // 角速度(deg/s)
+Vector geomag(3);                    // 地磁気(?)
 float press;                        // 気圧(hPa)
+
+Vector g(3);                        // 重力ベクトル
+Vector n(3);                        // 地磁気ベクトル
+Vector bias(3);                     // 地磁気センサのバイアスベクトル
+
 /********** private functions **********/
 void INT_func();                    // 割り込み用関数
+
 /********** main function     **********/
 
 int main() {
@@ -38,23 +49,31 @@
     if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!");        // mpu6050初期化
     if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!");       // hmc5883l初期化
     
-    timer.attach(&INT_func, freq);  // 割り込み有効化
+    timer.attach(&INT_func, Freq);  // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
+    
+    //重力ベクトルの初期化
+    g.SetComp(1, 0.0f);
+    g.SetComp(2, 0.0f);
+    g.SetComp(3, 1.0f);
     
     while(1) {
         
+        // 1秒おきにセンサーの出力をpcへ出力
+        wait(1.0f);
+        
         INT_flag = FALSE;           // 割り込みによる変数書き換えを阻止
         
         // センサ類の全出力値をPCに送信
-        printf("%f\t", acc[0]);
-        printf("%f\t", acc[1]);
-        printf("%f\t", acc[2]);
-        printf("%f\t", gyro[0]);
-        printf("%f\t", gyro[1]);
-        printf("%f\t", gyro[2]);
-        printf("%f\t", geomag[0]);
-        printf("%f\t", geomag[1]);
-        printf("%f\t", geomag[2]);
-        printf("%f\r\n", press);
+        pc.printf("%f,", acc.GetComp(1));
+        pc.printf("%f,", acc.GetComp(2));
+        pc.printf("%f,", acc.GetComp(3));
+        pc.printf("%f,", gyro.GetComp(1));
+        pc.printf("%f,", gyro.GetComp(2));
+        pc.printf("%f,", gyro.GetComp(3));
+        pc.printf("%f,", geomag.GetComp(1));
+        pc.printf("%f,", geomag.GetComp(2));
+        pc.printf("%f,", geomag.GetComp(3));
+        pc.printf("%f\r\n", press);
         
         INT_flag = TRUE;            // 割り込み許可
         
@@ -64,15 +83,24 @@
 void INT_func() {
     if(INT_flag == FALSE) {
         
+        // センサーの値を更新
         mpu.read();
         hmc.read();
         
         for(int i=0; i<3; i++) {
-            acc[i] = (float)mpu.data.value.acc[i] * ACC_LSB_TO_G * G_TO_MPSS;
-            gyro[i] = mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG;
-            geomag[i] = hmc.data.value[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(gyro, g);                            // Δg = ω × g
+        g = 0.9f * (g - delta_g * Freq) + 0.1f * acc.Normalize();   // 相補フィルタ
+        g = g.Normalize();
+        
+        Vector delta_n = Cross(gyro,n);
+        n = 0.9f * (n - delta_n * Freq) + 0.1f * geomag.Normalize();
+        n = n.Normalize();
+        
         // LPS25Hによる気圧の取得は10Hz
         lps_cnt = (lps_cnt+1)%10;
         if(lps_cnt == 0) {