Revision 3:40559ebef0f1, committed 2015-05-13

Comitter:

ojan

Date:

Wed May 13 04:02:27 2015 +0000

Parent:

2:4a6b46653abf

Commit message:

estimate gravity vector & geomagnetic vector

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC5883L.cpp	Wed May 13 04:02:27 2015 +0000
@@ -0,0 +1,38 @@
+#include "mbed.h"
+#include "myConstants.h"
+#include "HMC5883L.h"
+
+HMC5883L::HMC5883L(I2C* i2c) {
+    this->i2c = i2c;
+}
+
+HMC5883L::~HMC5883L() {
+    i2c = NULL;
+}
+
+int HMC5883L::init() {
+    return 1;
+}
+
+int HMC5883L::read() {
+    char cmd[2] = {0x02, 0x01};
+    int ret = i2c->write(hmc_addr, cmd, 2);
+    if(ret != 0) return 0;
+    
+    cmd[0] = 0x03;
+    i2c->write(hmc_addr, cmd, 1);
+    i2c->read(hmc_addr | 0x01, data.reg, 6, true);
+    
+    for(int i=0; i<3; i++) {
+        char temp = 0;
+        temp = data.reg[i*2];
+        data.reg[i*2] = data.reg[i*2+1];
+        data.reg[i*2+1] = temp;
+    }
+    
+    int16_t temp = data.value[1];
+    data.value[1] = data.value[2];
+    data.value[2] = temp;
+    
+    return 1;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC5883L.h	Wed May 13 04:02:27 2015 +0000
@@ -0,0 +1,32 @@
+#pragma once
+
+const static int hmc_addr = 0x3c;
+
+typedef union {
+    char reg[6];
+    struct {
+        uint8_t mag_x_L;
+        uint8_t mag_x_H;
+        uint8_t mag_y_L;
+        uint8_t mag_y_H;
+        uint8_t mag_z_L;
+        uint8_t mag_z_H;
+    } byte;
+    
+    int16_t value[3];
+    
+} HMC_DATA;
+
+class HMC5883L {
+public:
+    HMC5883L(I2C* i2c);
+    ~HMC5883L();
+    
+    int init();
+    int read();
+    
+    HMC_DATA data;
+    
+private:
+    I2C* i2c;
+};
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050.cpp	Wed May 13 04:02:27 2015 +0000
@@ -0,0 +1,38 @@
+#include "mbed.h"
+#include "myConstants.h"
+#include "MPU6050.h"
+
+MPU6050::MPU6050(I2C* i2c) {
+    this->i2c = i2c;
+}
+
+MPU6050::~MPU6050() {
+    i2c = NULL;
+}
+
+int MPU6050::init() {
+    char cmd[2] = {0x6b, 0x00};
+    int ret = i2c->write(mpu_addr, cmd, 2);
+    
+    if(ret != 0) return 0;
+    return 1;
+}
+
+int MPU6050::read() {
+    char cmd[1] = {0x3b};
+    int ret = i2c->write(mpu_addr, cmd, 1, true);
+    if(ret != 0) return 0;
+    
+    i2c->read(mpu_addr | 0x01, (char*)data.reg, 14, false);
+    
+    // データのHとLが逆に読み込まれているのでスワップする
+    for(int i=0; i<7; i++) {
+        uint8_t temp = 0;
+        temp = data.reg[i*2];
+        data.reg[i*2] = data.reg[i*2+1];
+        data.reg[i*2+1] = temp;
+    }
+    
+    return 1;
+    
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050.h	Wed May 13 04:02:27 2015 +0000
@@ -0,0 +1,47 @@
+#pragma once
+
+const static int mpu_addr = 0xd0;
+
+typedef union {
+    uint8_t reg[14]; 
+    struct {
+        uint8_t acc_x_L;
+        uint8_t acc_x_H;
+        uint8_t acc_y_L;
+        uint8_t acc_y_H;
+        uint8_t acc_z_L;
+        uint8_t acc_z_H;
+        
+        uint8_t T_L;
+        uint8_t T_H;
+        
+        uint8_t gyro_x_L;
+        uint8_t gyro_x_H;
+        uint8_t gyro_y_L;
+        uint8_t gyro_y_H;
+        uint8_t gyro_z_L;
+        uint8_t gyro_z_H;
+    } byte;
+    
+    struct {
+        int16_t acc[3];
+        
+        int16_t T;
+        
+        int16_t gyro[3];
+    } value;
+} MPU_DATA;
+
+class MPU6050 {
+public:
+    MPU6050(I2C* i2c);
+    ~MPU6050();
+    
+    int init();
+    int read();
+    
+    MPU_DATA data;
+    
+private:
+    I2C* i2c;
+};
\ No newline at end of file

--- a/Vector.cpp	Mon Apr 20 14:54:55 2015 +0000
+++ b/Vector.cpp	Wed May 13 04:02:27 2015 +0000
@@ -85,7 +85,7 @@
     return sqrt(norm);
 }
 
-Vector Vector::GetUnit() const {
+Vector Vector::Normalize() const {
     float norm = GetNorm();
     Vector temp(*this);
     for (int i = 0; i < dim; i++) {

--- a/Vector.h	Mon Apr 20 14:54:55 2015 +0000
+++ b/Vector.h	Wed May 13 04:02:27 2015 +0000
@@ -16,7 +16,7 @@
 
     void SetComp(int dimNo, float val);
     float GetNorm() const;
-    Vector GetUnit() const;
+    Vector Normalize() const;
 
     inline int GetDim() const {
         return dim;

--- a/main.cpp	Mon Apr 20 14:54:55 2015 +0000
+++ b/main.cpp	Wed May 13 04:02:27 2015 +0000
@@ -4,20 +4,23 @@
 #include "ErrorLogger.h"
 #include "Matrix.h"
 #include "Vector.h"
+#include "MPU6050.h"
+#include "HMC5883L.h"
 /********** private define **********/
 /********** private macro **********/
 /********** private typedef **********/
-/********** public variables **********/
-/********** public functions **********/
 /********** private variables **********/
+const static float dt = 0.01f;
 
 DigitalOut myled(LED1);                 // デバッグ用LEDのためのデジタル出力
 Serial pc(SERIAL_TX, SERIAL_RX);        // PC通信用シリアルポート
 I2C i2c(D14, D15);                  // mpu6050用I2Cオブジェクト
 Ticker INS_ticker;                      // 割り込み用タイマー
+MPU6050 mpu6050(&i2c);
+HMC5883L hmc5883l(&i2c);
 
-const int mpu6050_addr = 0xd0;          // mpu6050アドレス
-const int hmc5883l_addr = 0x3C;         // hmc5883lアドレス
+//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受信データ
@@ -27,7 +30,11 @@
 //int16_t mag[3] = {};                    // 地磁気
 Vector acc(3);
 Vector gyro(3);
+Vector mag(3);
 Vector g(3);
+Vector n(3);
+Vector v_acc(3);
+Vector v(3);
 float theta[2] = {};                    // ロール、ピッチ角
 
 char text[256];                         // デバッグ用文字列
@@ -41,23 +48,52 @@
 int main() {
     
     i2c.frequency(400000);                      // mpu6050との通信は400kHz
+    mpu6050.init();
+    hmc5883l.init();
     
     // 0x6bレジスタに0x00を書き込んでmpu6050のスリープモードを解除
-    cmd[0] = 0x6b;
+    /*cmd[0] = 0x6b;
     cmd[1] = 0x00;
     ret = i2c.write(mpu6050_addr, (char*)cmd, 2);
-    pc.printf("ret = %d ", ret);
+    pc.printf("ret = %d ", ret);*/
+    
+    // 地磁気センサの初期値を取得
+    hmc5883l.read();
+    mag.SetComp(1, (float)hmc5883l.data.value[0]);
+    mag.SetComp(2, (float)hmc5883l.data.value[1]);
+    mag.SetComp(3, (float)hmc5883l.data.value[2]);
+    
+    mag = mag.Normalize();
+    
+    v.SetComp(1, 0.0f);
+    v.SetComp(2, 0.0f);
+    v.SetComp(3, 0.0f);
+    
+    g.SetComp(1, 0.0f);
+    g.SetComp(2, 0.0f);
+    g.SetComp(3, 1.0f);
     
     // センサー値の取得・計算は割り込み関数内で行う。
     // 割り込み周期は10ms（10000μs）
-    INS_ticker.attach_us(&INS_IntFunc, 10000);    
+    INS_ticker.attach_us(&INS_IntFunc, 1000000 * dt);    
     
     while(1) {
        // メインループではひたすらLEDチカチカ
        myled = 1; // LED is ON
-       wait(0.2); // 200 ms
+       wait(0.05); // 50 ms
+       
+            pc.printf("%.3f\t", g.GetComp(1));
+            pc.printf("%.3f\t", g.GetComp(2));
+            pc.printf("%.3f\t", g.GetComp(3));
+            pc.printf("%.3f\t", n.GetComp(1));
+            pc.printf("%.3f\t", n.GetComp(2));
+            pc.printf("%.3f\t", n.GetComp(3));
+            pc.printf("%.3f\t", v.GetComp(1));
+            pc.printf("%.3f\t", v.GetComp(2));
+            pc.printf("%.3f\r\n", v.GetComp(3));
+            
        myled = 0; // LED is OFF
-       wait(1.0); // 1 sec
+       wait(0.05); // 1 sec
            
     }
 }
@@ -65,7 +101,7 @@
 void INS_IntFunc() {
         
         // 0x3bレジスタからデータの読み取りを行う
-        cmd[0] = 0x3b;
+        /*cmd[0] = 0x3b;
         ret = i2c.write(mpu6050_addr, (char*)cmd, 1, true);
         i2c.read(mpu6050_addr | 0x01, (char*)data, 14, false);
         
@@ -80,23 +116,65 @@
             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);
+        }*/
+        
+        mpu6050.read();
+        hmc5883l.read();
+        
+        for(int i=0; i<3; i++) {
+            acc.SetComp(i+1, (float)mpu6050.data.value.acc[i] * ACC_LSB_TO_G);
+            gyro.SetComp(i+1, (float)mpu6050.data.value.gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD);
+            mag.SetComp(i+1, (float)hmc5883l.data.value[i]);
         }
         
+        //acc = acc.Normalize();      // 欲しいのは方向のみなので単位ベクトル化
+        //mag = mag.Normalize();      // 欲しいのは方向のみなので単位ベクトル化
+        
+        /*pc.printf("%.4f\t", acc.GetComp(1));
+        pc.printf("%.4f\t", acc.GetComp(2));
+        pc.printf("%.4f\t", acc.GetComp(3));
+        pc.printf("%.4f\t", gyro.GetComp(1));
+        pc.printf("%.4f\t", gyro.GetComp(2));
+        pc.printf("%.4f\t", gyro.GetComp(3));
+        pc.printf("%.4f\t", mag.GetComp(1));
+        pc.printf("%.4f\t", mag.GetComp(2));
+        pc.printf("%.4f\r\n", mag.GetComp(3));
+        */
         // 重力ベクトルを推定
         {
-            acc = acc.GetUnit();            // 欲しいのは方向のみなので、単位ベクトル化
-            Vector delta = Cross(gyro, g);  // Δg = ω × g
+            Vector delta_g = Cross(gyro, g);  // Δg = ω × g
+            Vector delta_n = Cross(gyro, n);  // Δf = ω × f
+            
             
             // 相補フィルタを使ってみる
-            g = 0.9f * (g + 0.01f * delta) + 0.1f * acc;
-            g = g.GetUnit();
+            //g = g + delta * dt;
+            g = 0.9f * (g + delta_g * dt) + 0.1f * acc.Normalize();
+            g = g.Normalize();
+            n = 0.9f * (n + delta_n * dt) + 0.1f * mag.Normalize();
+            n = n.Normalize();
+            
+            v_acc = G_TO_MPSS * (acc - (acc * g) * g);
+            
+            v += v_acc * dt;
             
             // 推定結果をPCに送信
-            pc.printf("%.4f\t", g.GetComp(1));
-            pc.printf("%.4f\t", g.GetComp(2));
-            pc.printf("%.4f\r\n", g.GetComp(3));
+            /*pc.printf("%.3f\t", g.GetComp(1));
+            pc.printf("%.3f\t", g.GetComp(2));
+            pc.printf("%.3f\t", g.GetComp(3));
+            pc.printf("%.3f\t", n.GetComp(1));
+            pc.printf("%.3f\t", n.GetComp(2));
+            pc.printf("%.3f\t", n.GetComp(3));
+            pc.printf("%.3f\t", v.GetComp(1));
+            pc.printf("%.3f\t", v.GetComp(2));
+            pc.printf("%.3f\r\n", v.GetComp(3));*/
             
-        }       
+            
+            
+            //pc.printf("%f\t", (float)mpu6050.data.value.gyro[0] * GYRO_LSB_TO_DEG);
+            //pc.printf("%f\t", (float)mpu6050.data.value.gyro[1] * GYRO_LSB_TO_DEG);
+            //pc.printf("%f\r\n", (float)mpu6050.data.value.gyro[2] * GYRO_LSB_TO_DEG);
+            
+        }
         
         /*
         // 各センサー値からセンサーの姿勢・角速度を計算

--- a/mbed.bld	Mon Apr 20 14:54:55 2015 +0000
+++ b/mbed.bld	Wed May 13 04:02:27 2015 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/433970e64889
\ No newline at end of file
+http://mbed.org/users/mbed_official/code/mbed/builds/8ab26030e058
\ No newline at end of file

--- a/myConstants.h	Mon Apr 20 14:54:55 2015 +0000
+++ b/myConstants.h	Wed May 13 04:02:27 2015 +0000
@@ -5,4 +5,6 @@
 #define RAD_TO_DEG          57.295779513f           // 180 / π
 #define DEG_TO_RAD          0.0174532925f           // π / 180
 #define ACC_LSB_TO_G        0.00006103515625f       // g/LSB
-#define GYRO_LSB_TO_DEG     0.00763358778f          // deg/LSB
\ No newline at end of file
+#define G_TO_MPSS           9.8f                    // (m/s^2)/g
+//#define GYRO_LSB_TO_DEG     0.0152671755f          // deg/LSB (1/65.5
+#define GYRO_LSB_TO_DEG     0.00763358778f          // deg/LSB (1/131
\ No newline at end of file