File content as of revision 2:d2b60a1d0cd9:

#include "mbed.h"
#include "MPU6050.h"
#include "HMC5883L.h"
#include "LPS25H.h"
#include "GMS6_CR6.h"
#include "ErrorLogger.h"
#include "Vector.h"
#include "myConstants.h"

/********** private define    **********/
#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      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）

int lps_cnt = 0;                    // 気圧センサ読み取りカウント
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() {
    
    i2c.frequency(400000);          // I2Cの通信速度を400kHzに設定
    
    if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!");        // mpu6050初期化
    if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!");       // hmc5883l初期化
    
    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に送信
        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;            // 割り込み許可
        
    }
    
    /* ---------- ↑↑↑ ここまでメインループ ↑↑↑ ---------- */
}

void INT_func() {
    if(INT_flag == FALSE) {
        
        // センサーの値を更新
        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(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) {
            press = (float)lps.pressure() * PRES_LSB_TO_HPA;
        }
    }
}