Diff: main.cpp

Revision:

2:4a6b46653abf

Parent:

1:2eca9b376580

Child:

3:40559ebef0f1

--- a/main.cpp	Thu Apr 16 08:51:04 2015 +0000
+++ b/main.cpp	Mon Apr 20 14:54:55 2015 +0000
@@ -1,6 +1,11 @@
 #include "mbed.h"
+#include "myConstants.h"
 #include "toString.h"
 #include "ErrorLogger.h"
+#include "Matrix.h"
+#include "Vector.h"
+/********** private define **********/
+/********** private macro **********/
 /********** private typedef **********/
 /********** public variables **********/
 /********** public functions **********/
@@ -8,16 +13,21 @@
 
 DigitalOut myled(LED1);                 // デバッグ用LEDのためのデジタル出力
 Serial pc(SERIAL_TX, SERIAL_RX);        // PC通信用シリアルポート
-I2C mpu6050(D14, D15);                  // mpu6050用I2Cオブジェクト
+I2C i2c(D14, D15);                  // mpu6050用I2Cオブジェクト
 Ticker INS_ticker;                      // 割り込み用タイマー
 
-const int addr = 0xd0;                  // mpu6050アドレス
+const int mpu6050_addr = 0xd0;          // mpu6050アドレス
+const int hmc5883l_addr = 0x3C;         // hmc5883lアドレス
 volatile int ret = 0;                   // I2C関数の返り値保存用
 uint8_t cmd[2] = {};                    // I2C送信データ
 uint8_t data[14] = {};                  // I2C受信データ
 uint16_t Tempr = 0;                     // 温度
-int16_t acc[3] = {};                    // 加速度
-int16_t gyro[3] = {};                   // 角速度
+//int16_t acc[3] = {};                    // 加速度
+//int16_t gyro[3] = {};                   // 角速度
+//int16_t mag[3] = {};                    // 地磁気
+Vector acc(3);
+Vector gyro(3);
+Vector g(3);
 float theta[2] = {};                    // ロール、ピッチ角
 
 char text[256];                         // デバッグ用文字列
@@ -30,17 +40,17 @@
 
 int main() {
     
-    mpu6050.frequency(400000);                      // mpu6050との通信は400kHz
+    i2c.frequency(400000);                      // mpu6050との通信は400kHz
     
     // 0x6bレジスタに0x00を書き込んでmpu6050のスリープモードを解除
     cmd[0] = 0x6b;
     cmd[1] = 0x00;
-    ret = mpu6050.write(addr, (char*)cmd, 2);
+    ret = i2c.write(mpu6050_addr, (char*)cmd, 2);
     pc.printf("ret = %d ", ret);
     
     // センサー値の取得・計算は割り込み関数内で行う。
     // 割り込み周期は10ms（10000μs）
-    INS_ticker.attach_us(&INS_IntFunc, 10000);
+    INS_ticker.attach_us(&INS_IntFunc, 10000);    
     
     while(1) {
        // メインループではひたすらLEDチカチカ
@@ -56,29 +66,51 @@
         
         // 0x3bレジスタからデータの読み取りを行う
         cmd[0] = 0x3b;
-        ret = mpu6050.write(addr, (char*)cmd, 1, true);
-        mpu6050.read(addr | 0x01, (char*)data, 14, false);
+        ret = i2c.write(mpu6050_addr, (char*)cmd, 1, true);
+        i2c.read(mpu6050_addr | 0x01, (char*)data, 14, false);
         
         // 各データを加速度、角速度にそれぞれ突っ込む
         for(int i=0; i<3; i++) {
-            acc[i] = ((int16_t)data[i*2])<<8 | ((int16_t)data[i*2+1]);
+            int16_t acc_temp = 0;
+            acc_temp = ((int16_t)data[i*2])<<8 | ((int16_t)data[i*2+1]);
+            acc.SetComp(i+1, -(float)acc_temp * ACC_LSB_TO_G);
         }
         Tempr = ((int16_t)data[6])<<8 | ((int16_t)data[7]);
         for(int i=4; i<7; i++) {
-            gyro[i-4] = ((int16_t)data[i*2])<<8 | ((int16_t)data[i*2+1]);
+            int16_t gyro_temp = 0;
+            gyro_temp = ((int16_t)data[i*2])<<8 | ((int16_t)data[i*2+1]);
+            gyro.SetComp(i-3, (float)gyro_temp * GYRO_LSB_TO_DEG * DEG_TO_RAD);
         }
         
+        // 重力ベクトルを推定
+        {
+            acc = acc.GetUnit();            // 欲しいのは方向のみなので、単位ベクトル化
+            Vector delta = Cross(gyro, g);  // Δg = ω × g
+            
+            // 相補フィルタを使ってみる
+            g = 0.9f * (g + 0.01f * delta) + 0.1f * acc;
+            g = g.GetUnit();
+            
+            // 推定結果をPCに送信
+            pc.printf("%.4f\t", g.GetComp(1));
+            pc.printf("%.4f\t", g.GetComp(2));
+            pc.printf("%.4f\r\n", g.GetComp(3));
+            
+        }       
+        
+        /*
         // 各センサー値からセンサーの姿勢・角速度を計算
-        float roll_acc = (180.0f * atan2((float)acc[0], (float)acc[2]) / 3.1415f);
+        float roll_acc = (atan2((float)acc[0], (float)acc[2]) * RAD_TO_DEG);
         float roll_ratio_gyro = (float)gyro[1] / 65.5f;
-        float pitch_acc = (180.0f * atan2((float)acc[1], (float)acc[2]) / 3.1415f);
+        float pitch_acc = (atan2((float)acc[1], (float)acc[2]) * RAD_TO_DEG);
         float pitch_ratio_gyro = (float)gyro[0] / 65.5f;
         
         // 相補フィルタを用いて角度を更新
-        theta[0] = 0.98f * (theta[0] + roll_ratio_gyro * 0.01f) + 0.02f * roll_acc;
+        theta[0] = 0.98f * (theta[0] - roll_ratio_gyro * 0.01f) + 0.02f * roll_acc;
         theta[1] = 0.98f * (theta[1] + pitch_ratio_gyro * 0.01f) + 0.02f * pitch_acc;
         
         // 推定された角度をパソコンに送信
         pc.printf("%.4f\t", theta[0]);
         pc.printf("%.4f\r\n", theta[1]);
+        */
 }
\ No newline at end of file