File content as of revision 4:8df0fc5dfd81:

#include "mbed.h"
#include "myConstants.h"
#include "toString.h"
#include "ErrorLogger.h"
#include "Matrix.h"
#include "Vector.h"
#include "MPU6050.h"
#include "HMC5883L.h"
/********** private define **********/
/********** private macro **********/
/********** private typedef **********/
/********** private variables **********/
const static float dt = 1.0f;

DigitalOut myled(LED1);                 // デバッグ用LEDのためのデジタル出力
Serial pc(USBTX, USBRX);        // PC通信用シリアルポート
I2C i2c(P0_13, P0_15);                  // mpu6050用I2Cオブジェクト
Ticker INS_ticker;                      // 割り込み用タイマー
MPU6050 mpu6050(&i2c);
HMC5883L hmc5883l(&i2c);

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 mag[3] = {};     
               // 地磁気
Vector Vacc(3);
Vector Vgyro(3);
Vector Vmag(3);
Vector g(3);
Vector n(3);
Vector v_acc(3);
Vector v(3);
//float theta[2] = {};                    // ロール、ピッチ角
float pitch = 0.0, roll = 0.0;
char text[256];                         // デバッグ用文字列

/********** private functions **********/

void INS_IntFunc();                     // センサー値取得用割り込み関数

/********** main function **********/

int main() {
    
    i2c.frequency(400000);                      // mpu6050との通信は400kHz
    mpu6050.init();
    hmc5883l.init();
    
    // 0x6bレジスタに0x00を書き込んでmpu6050のスリープモードを解除
    cmd[0] = 0x6b;
    cmd[1] = 0x00;
    ret = i2c.write(mpu6050_addr, (char*)cmd, 2);
    //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, 1000000 * dt);    
    
    while(1) {
       // メインループではひたすらLEDチカチカ
       myled = 0; // LED is ON
       wait(10); // 50 ms
            //pc.printf("testtest\n");
            //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(10); // 1 sec
           
    }
}

void INS_IntFunc() {
        
        // 0x3bレジスタからデータの読み取りを行う
        cmd[0] = 0x3b;
        ret = i2c.write(mpu6050_addr, (char*)cmd, 1, true);
        i2c.read(mpu6050_addr | 0x01, (char*)data, 14, false);
        
        mpu6050.read();
        hmc5883l.read();
        
        // 各データを加速度、角速度にそれぞれ突っ込む
        for(int i=0; i<3; i++) {
            int16_t acc_temp = 0;
            acc_temp = ((int16_t)data[i*2])<<8 | ((int16_t)data[i*2+1]);
            //Vacc.SetComp(i+1, -(float)acc_temp * ACC_LSB_TO_G);
            Vacc.SetComp(i+1, -(float)acc_temp);
        }
        Tempr = ((int16_t)data[6])<<8 | ((int16_t)data[7]);
        for(int i=4; i<7; i++) {
            int16_t gyro_temp = 0;
            gyro_temp = ((int16_t)data[i*2])<<8 | ((int16_t)data[i*2+1]);
            Vgyro.SetComp(i-3, (float)gyro_temp * GYRO_LSB_TO_DEG * DEG_TO_RAD);
        }
        
        //mpu6050.read();
//        hmc5883l.read();
        
        for(int i=0; i<3; i++) {
            //Vacc.SetComp(i+1, (float)acc[i]);
            Vacc.SetComp(i+1, (float)acc[i] * ACC_LSB_TO_G);
            Vgyro.SetComp(i+1, (float)gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD);
            //Vmag.SetComp(i+1, (float)hmc5883l.data.value[i]);
        }
        
        Vacc = Vacc.Normalize();      // 欲しいのは方向のみなので単位ベクトル化
        Vmag = Vmag.Normalize();      // 欲しいのは方向のみなので単位ベクトル化
        
       // pc.printf("%.4f\t", Vacc.GetComp(1));
//        pc.printf("%.4f\t", Vacc.GetComp(2));
//        pc.printf("%.4f\t", Vacc.GetComp(3));
//        pc.printf("%.4f\t", Vgyro.GetComp(1));
//        pc.printf("%.4f\t", Vgyro.GetComp(2));
//        pc.printf("%.4f\t", Vgyro.GetComp(3));
//        pc.printf("%.4f\t", Vmag.GetComp(1));
//        pc.printf("%.4f\t", Vmag.GetComp(2));
//        pc.printf("%.4f\r\n", Vmag.GetComp(3));
//        
        // 重力ベクトルを推定
        
            Vector delta_g = Cross(Vgyro, g);  // Δg = ω × g
            Vector delta_n = Cross(Vgyro, n);  // Δf = ω × f
            
            
            // 相補フィルタを使ってみる
            //g = g + delta * dt;
            g = 0.9f * (g + delta_g * dt) + 0.1f * Vacc.Normalize();
            g = g.Normalize();
            n = 0.9f * (n + delta_n * dt) + 0.1f * Vmag.Normalize();
            n = n.Normalize();
            
            v_acc = G_TO_MPSS * (Vacc - (Vacc * g) * g);
            
            v += v_acc * dt;
            
            // 推定結果をPCに送信
           // 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);
//            
        
        
        
        // 各センサー値からセンサーの姿勢・角速度を計算
        
        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 = (atan2((float)acc[1], (float)acc[2]) * RAD_TO_DEG);
        float pitch_ratio_gyro = (float)gyro[0] / 65.5f;
        
       // for(int i = 0; i < 3; i++){
//            g.
        // 相補フィルタを用いて角度を更新
        roll = 0.98f * (theta[0] - roll_ratio_gyro * 0.01f) + 0.02f * roll_acc;
        pitch = 0.98f * (theta[1] + pitch_ratio_gyro * 0.01f) + 0.02f * pitch_acc;

        //pitch = 0.98f * atan((float)g.GetComp(1) / sqrt((float)g.GetComp(2)^2 + (float)g.GetComp(3)^2));
//        roll = 0.98f * atan((float)g.GetComp(2) / sqrt((float)g.GetComp(1)^2 + (float)g.GetComp(3)^2));        
        // 推定された角度をパソコンに送信
        //pc.printf("%.4f\t", Vacc.GetComp(1));
       // pc.printf("%.4f\t", atan2((float)acc[0], (float)acc[2]));
        pc.printf("%.4f\t", theta[0]);
        pc.printf("%.4f\r\n", theta[1]);
//        
}