hiroya taura
MPU6050のサンプルプログラム２
Dependencies: ConfigFile SDFileSystem mbed
Fork of
LAURUS_program
by 
LAURUS
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());
}
}