MPUとHMCでうごくかもver
Dependencies: ConfigFile SDFileSystem mbed
Fork of LAURUS_program by
main.cpp
- Committer:
- ojan
- Date:
- 2015-05-24
- Revision:
- 2:d2b60a1d0cd9
- Parent:
- 1:6cd6d2760856
- Child:
- 3:5358a691a100
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; } } }