MPUとHMCでうごくかもver
Dependencies: ConfigFile SDFileSystem mbed
Fork of LAURUS_program by
Diff: main.cpp
- Revision:
- 4:45dc5590abc0
- Parent:
- 3:5358a691a100
- Child:
- 5:182f6356bce1
--- a/main.cpp Sat May 30 18:08:34 2015 +0000 +++ b/main.cpp Thu Jun 11 15:43:07 2015 +0000 @@ -18,7 +18,8 @@ /********** private typedef **********/ /********** private variables **********/ -I2C i2c(PB_9, PB_8); // I2Cポート +DigitalOut myled(LED1); // デバッグ用LEDのためのデジタル出力 +I2C i2c(PB_9, PB_8); // I2Cポート MPU6050 mpu(&i2c); // 加速度・角速度センサ HMC5883L hmc(&i2c); // 地磁気センサ LPS25H lps(&i2c); // 気圧センサ @@ -26,18 +27,24 @@ Serial pc(SERIAL_TX, SERIAL_RX); // PC通信用シリアルポート GMS6_CR6 gms(&gps, &pc); // GPS Ticker INT_timer; // 割り込みタイマー -//Log logger(PA_9, PA_10, PB_5, PB_4, PB_3, PB_10, "sd"); // ロガー(microSD、XBee) +Log logger(PA_9, PA_10, PC_1, PB_5, PB_4, PB_3, PB_10); // ロガー(microSD、XBee) +DigitalIn cts(PC_1); const float dt = 0.1f; // 割り込み周期(s) int lps_cnt = 0; // 気圧センサ読み取りカウント uint8_t INT_flag = TRUE; // 割り込み可否フラグ +Vector raw_acc(3); // 加速度(m/s^2) 生 +Vector raw_gyro(3); // 角速度(deg/s) 生 +Vector raw_geomag(3); // 地磁気(?) 生 +float raw_press; // 気圧(hPa) 生 Vector acc(3); // 加速度(m/s^2) Vector gyro(3); // 角速度(deg/s) Vector geomag(3); // 地磁気(?) float press; // 気圧(hPa) -Vector g(3); // 重力ベクトル +Vector raw_g(3); // 重力ベクトル 生 +Vector g(3); // 重力ベクトル 生 //Vector n(3); // 地磁気ベクトル /* ----- Kalman Filter ----- */ @@ -57,8 +64,8 @@ char data[1024] = {}; /********** private functions **********/ -void KalmanInit(); -void KalmanUpdate(); +void KalmanInit(); // カルマンフィルタ初期化 +void KalmanUpdate(); // カルマンフィルタ更新 void INT_func(); // 割り込み用関数 void toString(Matrix& m); void toString(Vector& v); @@ -72,57 +79,44 @@ if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!"); // mpu6050初期化 if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!"); // hmc5883l初期化 - //char* title = "log data\r\n"; // ログのタイトル行 - //if(!logger.initialize_sdlog(title)) return 0; // ログファイル初期設定 + char* title = "log data\r\n"; // ログのタイトル行 + if(!logger.initialize_sdlog(title)) return 0; // ログファイル初期設定 KalmanInit(); INT_timer.attach(&INT_func, dt); // 割り込み有効化(Freq = 0.01fなので、10msおきの割り込み) //重力ベクトルの初期化 - g.SetComp(1, 0.0f); - g.SetComp(2, 0.0f); - g.SetComp(3, 1.0f); + raw_g.SetComp(1, 0.0f); + raw_g.SetComp(2, 0.0f); + raw_g.SetComp(3, 1.0f); /* ---------- ↓↓↓ ここからメインループ ↓↓↓ ---------- */ while(1) { - + timer.stop(); + timer.reset(); + timer.start(); // 0.1秒おきにセンサーの出力をpcへ出力 - wait(0.1f); + myled = 1; // LED is ON + wait(0.05f); // 50 ms + myled = 0; // LED is OFF INT_flag = FALSE; // 割り込みによる変数書き換えを阻止 - // センサ類の全出力値をPCに送信 - /* - pc.printf("%.3f,", acc.GetComp(1)); - pc.printf("%.3f,", acc.GetComp(2)); - pc.printf("%.3f,", acc.GetComp(3)); - pc.printf("%.3f,", gyro.GetComp(1)); - pc.printf("%.3f,", gyro.GetComp(2)); - pc.printf("%.3f,", gyro.GetComp(3)); - pc.printf("%.3f,", geomag.GetComp(1)); - pc.printf("%.3f,", geomag.GetComp(2)); - pc.printf("%.3f\r\n", geomag.GetComp(3)); - */ - //pc.printf("%.3f\r\n", press); - - - pc.printf("%.3f,", geomag.GetComp(1)); - pc.printf("%.3f,", geomag.GetComp(2)); - pc.printf("%.3f,", geomag.GetComp(3)); - pc.printf("%.3f,", post_x.GetComp(1)); - pc.printf("%.3f,", post_x.GetComp(2)); - pc.printf("%.3f\r\n", post_x.GetComp(3)); - - /* - sprintf(data, "%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", + sprintf(data, "%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", acc.GetComp(1), acc.GetComp(2), acc.GetComp(3), gyro.GetComp(1), gyro.GetComp(2), gyro.GetComp(3), - geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3), press); + geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3), + press, gms.longitude, gms.latitude); logger.puts(data); - */ + INT_flag = TRUE; // 割り込み許可 + pc.printf("time: %.3f[s]\r\n", (float)timer.read_ms()/1000.0f); + + // ループはきっかり1秒ごと + while(timer.read_ms() < 1000); + } /* ---------- ↑↑↑ ここまでメインループ ↑↑↑ ---------- */ @@ -138,9 +132,9 @@ // 状態方程式のヤコビアンの初期値を代入(時間変化あり) float f[36] = { - 1.0f, gyro.GetComp(3)*dt, -gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f, - -gyro.GetComp(3)*dt, 1.0f, gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f, - gyro.GetComp(2)*dt, -gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f, + 1.0f, raw_gyro.GetComp(3)*dt, -raw_gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f, + -raw_gyro.GetComp(3)*dt, 1.0f, raw_gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f, + raw_gyro.GetComp(2)*dt, -raw_gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f @@ -161,9 +155,9 @@ void KalmanUpdate() { // ヤコビアンの更新 float f[36] = { - 1.0f, gyro.GetComp(3)*dt, -gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f, - -gyro.GetComp(3)*dt, 1.0f, gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f, - gyro.GetComp(2)*dt, -gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f, + 1.0f, raw_gyro.GetComp(3)*dt, -raw_gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f, + -raw_gyro.GetComp(3)*dt, 1.0f, raw_gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f, + raw_gyro.GetComp(2)*dt, -raw_gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f @@ -186,42 +180,47 @@ K = pri_P * H.Transpose() * inv; // 事後推定値の更新 - post_x = pri_x + K * (geomag - H * pri_x); + post_x = pri_x + K * (raw_geomag - H * pri_x); // 事後誤差分散行列の更新 post_P = (I - K * H) * pri_P; } void INT_func() { - if(INT_flag != FALSE) { + // センサーの値を更新 + mpu.read(); + hmc.read(); + + for(int i=0; i<3; i++) { + raw_acc.SetComp(i+1, (float)mpu.data.value.acc[i] * ACC_LSB_TO_G); + raw_gyro.SetComp(i+1, (float)mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD); + raw_geomag.SetComp(i+1, (float)hmc.data.value[i] * MAG_LSB_TO_GAUSS); + } - timer.reset(); - timer.start(); - // センサーの値を更新 - 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(raw_gyro, raw_g); // Δg = ω × g + raw_g = 0.9f * (raw_g - delta_g * dt) + 0.1f * raw_acc.Normalize(); // 相補フィルタ + raw_g = raw_g.Normalize(); + + KalmanUpdate(); - Vector delta_g = Cross(gyro, g); // Δg = ω × g - g = 0.9f * (g - delta_g * dt) + 0.1f * acc.Normalize(); // 相補フィルタ - g = g.Normalize(); - - - KalmanUpdate(); - + // LPS25Hによる気圧の取得は10Hz + lps_cnt = (lps_cnt+1)%10; + if(lps_cnt == 0) { + raw_press = (float)lps.pressure() * PRES_LSB_TO_HPA; + } + //pc.printf("%d(us)\r\n", timer.read_us()); + + //pc.printf("PC_1 = %d\r\n", (int)cts); + //pc.printf("test\r\n"); + + if(INT_flag != FALSE) { + g = raw_g; + for(int i=0; i<3; i++) { + geomag.SetComp(i+1, post_x.GetComp(i+1)); + } + acc = raw_acc; + gyro = raw_gyro; + press = raw_press; - // LPS25Hによる気圧の取得は10Hz - lps_cnt = (lps_cnt+1)%10; - if(lps_cnt == 0) { - press = (float)lps.pressure() * PRES_LSB_TO_HPA; - } - - timer.stop(); - //pc.printf("%d(us)\r\n", timer.read_us()); } }