MPUとHMCでうごくかもver
Dependencies: ConfigFile SDFileSystem mbed
Fork of LAURUS_program by
main.cpp@2:d2b60a1d0cd9, 2015-05-24 (annotated)
- Committer:
- ojan
- Date:
- Sun May 24 17:44:11 2015 +0000
- Revision:
- 2:d2b60a1d0cd9
- Parent:
- 1:6cd6d2760856
- Child:
- 3:5358a691a100
Send all sensor's data to PC
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ojan | 0:bc6f14fc60c7 | 1 | #include "mbed.h" |
ojan | 0:bc6f14fc60c7 | 2 | #include "MPU6050.h" |
ojan | 0:bc6f14fc60c7 | 3 | #include "HMC5883L.h" |
ojan | 0:bc6f14fc60c7 | 4 | #include "LPS25H.h" |
ojan | 1:6cd6d2760856 | 5 | #include "GMS6_CR6.h" |
ojan | 0:bc6f14fc60c7 | 6 | #include "ErrorLogger.h" |
ojan | 0:bc6f14fc60c7 | 7 | #include "Vector.h" |
ojan | 0:bc6f14fc60c7 | 8 | #include "myConstants.h" |
ojan | 0:bc6f14fc60c7 | 9 | |
ojan | 0:bc6f14fc60c7 | 10 | /********** private define **********/ |
ojan | 0:bc6f14fc60c7 | 11 | #define TRUE 1 |
ojan | 0:bc6f14fc60c7 | 12 | #define FALSE 0 |
ojan | 0:bc6f14fc60c7 | 13 | /********** private macro **********/ |
ojan | 1:6cd6d2760856 | 14 | |
ojan | 0:bc6f14fc60c7 | 15 | /********** private typedef **********/ |
ojan | 1:6cd6d2760856 | 16 | |
ojan | 0:bc6f14fc60c7 | 17 | /********** private variables **********/ |
ojan | 1:6cd6d2760856 | 18 | I2C i2c(I2C_SDA, I2C_SCL); // I2Cポート |
ojan | 1:6cd6d2760856 | 19 | MPU6050 mpu(&i2c); // 加速度・角速度センサ |
ojan | 1:6cd6d2760856 | 20 | HMC5883L hmc(&i2c); // 地磁気センサ |
ojan | 1:6cd6d2760856 | 21 | LPS25H lps(&i2c); // 気圧センサ |
ojan | 1:6cd6d2760856 | 22 | Serial gps(D8, D2); // GPS通信用シリアルポート |
ojan | 1:6cd6d2760856 | 23 | Serial pc(SERIAL_TX, SERIAL_RX); // PC通信用シリアルポート |
ojan | 1:6cd6d2760856 | 24 | GMS6_CR6 gms(&gps, &pc); // GPS |
ojan | 1:6cd6d2760856 | 25 | Ticker timer; // 割り込みタイマー |
ojan | 0:bc6f14fc60c7 | 26 | |
ojan | 1:6cd6d2760856 | 27 | const float Freq = 0.01f; // 割り込み周期(s) |
ojan | 0:bc6f14fc60c7 | 28 | |
ojan | 0:bc6f14fc60c7 | 29 | int lps_cnt = 0; // 気圧センサ読み取りカウント |
ojan | 1:6cd6d2760856 | 30 | uint8_t INT_flag = FALSE; // 割り込み可否フラグ |
ojan | 1:6cd6d2760856 | 31 | Vector acc(3); // 加速度(m/s^2) |
ojan | 1:6cd6d2760856 | 32 | Vector gyro(3); // 角速度(deg/s) |
ojan | 1:6cd6d2760856 | 33 | Vector geomag(3); // 地磁気(?) |
ojan | 0:bc6f14fc60c7 | 34 | float press; // 気圧(hPa) |
ojan | 1:6cd6d2760856 | 35 | |
ojan | 1:6cd6d2760856 | 36 | Vector g(3); // 重力ベクトル |
ojan | 1:6cd6d2760856 | 37 | Vector n(3); // 地磁気ベクトル |
ojan | 1:6cd6d2760856 | 38 | Vector bias(3); // 地磁気センサのバイアスベクトル |
ojan | 1:6cd6d2760856 | 39 | |
ojan | 0:bc6f14fc60c7 | 40 | /********** private functions **********/ |
ojan | 0:bc6f14fc60c7 | 41 | void INT_func(); // 割り込み用関数 |
ojan | 1:6cd6d2760856 | 42 | |
ojan | 0:bc6f14fc60c7 | 43 | /********** main function **********/ |
ojan | 0:bc6f14fc60c7 | 44 | |
ojan | 0:bc6f14fc60c7 | 45 | int main() { |
ojan | 0:bc6f14fc60c7 | 46 | |
ojan | 0:bc6f14fc60c7 | 47 | i2c.frequency(400000); // I2Cの通信速度を400kHzに設定 |
ojan | 0:bc6f14fc60c7 | 48 | |
ojan | 0:bc6f14fc60c7 | 49 | if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!"); // mpu6050初期化 |
ojan | 0:bc6f14fc60c7 | 50 | if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!"); // hmc5883l初期化 |
ojan | 0:bc6f14fc60c7 | 51 | |
ojan | 1:6cd6d2760856 | 52 | timer.attach(&INT_func, Freq); // 割り込み有効化(Freq = 0.01fなので、10msおきの割り込み) |
ojan | 1:6cd6d2760856 | 53 | |
ojan | 1:6cd6d2760856 | 54 | //重力ベクトルの初期化 |
ojan | 1:6cd6d2760856 | 55 | g.SetComp(1, 0.0f); |
ojan | 1:6cd6d2760856 | 56 | g.SetComp(2, 0.0f); |
ojan | 1:6cd6d2760856 | 57 | g.SetComp(3, 1.0f); |
ojan | 0:bc6f14fc60c7 | 58 | |
ojan | 2:d2b60a1d0cd9 | 59 | /* ---------- ↓↓↓ ここからメインループ ↓↓↓ ---------- */ |
ojan | 0:bc6f14fc60c7 | 60 | while(1) { |
ojan | 0:bc6f14fc60c7 | 61 | |
ojan | 1:6cd6d2760856 | 62 | // 1秒おきにセンサーの出力をpcへ出力 |
ojan | 1:6cd6d2760856 | 63 | wait(1.0f); |
ojan | 1:6cd6d2760856 | 64 | |
ojan | 0:bc6f14fc60c7 | 65 | INT_flag = FALSE; // 割り込みによる変数書き換えを阻止 |
ojan | 0:bc6f14fc60c7 | 66 | |
ojan | 0:bc6f14fc60c7 | 67 | // センサ類の全出力値をPCに送信 |
ojan | 1:6cd6d2760856 | 68 | pc.printf("%f,", acc.GetComp(1)); |
ojan | 1:6cd6d2760856 | 69 | pc.printf("%f,", acc.GetComp(2)); |
ojan | 1:6cd6d2760856 | 70 | pc.printf("%f,", acc.GetComp(3)); |
ojan | 1:6cd6d2760856 | 71 | pc.printf("%f,", gyro.GetComp(1)); |
ojan | 1:6cd6d2760856 | 72 | pc.printf("%f,", gyro.GetComp(2)); |
ojan | 1:6cd6d2760856 | 73 | pc.printf("%f,", gyro.GetComp(3)); |
ojan | 1:6cd6d2760856 | 74 | pc.printf("%f,", geomag.GetComp(1)); |
ojan | 1:6cd6d2760856 | 75 | pc.printf("%f,", geomag.GetComp(2)); |
ojan | 1:6cd6d2760856 | 76 | pc.printf("%f,", geomag.GetComp(3)); |
ojan | 1:6cd6d2760856 | 77 | pc.printf("%f\r\n", press); |
ojan | 0:bc6f14fc60c7 | 78 | |
ojan | 0:bc6f14fc60c7 | 79 | INT_flag = TRUE; // 割り込み許可 |
ojan | 0:bc6f14fc60c7 | 80 | |
ojan | 0:bc6f14fc60c7 | 81 | } |
ojan | 2:d2b60a1d0cd9 | 82 | |
ojan | 2:d2b60a1d0cd9 | 83 | /* ---------- ↑↑↑ ここまでメインループ ↑↑↑ ---------- */ |
ojan | 0:bc6f14fc60c7 | 84 | } |
ojan | 0:bc6f14fc60c7 | 85 | |
ojan | 0:bc6f14fc60c7 | 86 | void INT_func() { |
ojan | 0:bc6f14fc60c7 | 87 | if(INT_flag == FALSE) { |
ojan | 0:bc6f14fc60c7 | 88 | |
ojan | 1:6cd6d2760856 | 89 | // センサーの値を更新 |
ojan | 0:bc6f14fc60c7 | 90 | mpu.read(); |
ojan | 0:bc6f14fc60c7 | 91 | hmc.read(); |
ojan | 0:bc6f14fc60c7 | 92 | |
ojan | 0:bc6f14fc60c7 | 93 | for(int i=0; i<3; i++) { |
ojan | 1:6cd6d2760856 | 94 | acc.SetComp(i+1, (float)mpu.data.value.acc[i] * ACC_LSB_TO_G); |
ojan | 1:6cd6d2760856 | 95 | gyro.SetComp(i+1, (float)mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD); |
ojan | 1:6cd6d2760856 | 96 | geomag.SetComp(i+1, (float)hmc.data.value[i] * MAG_LSB_TO_GAUSS); |
ojan | 0:bc6f14fc60c7 | 97 | } |
ojan | 0:bc6f14fc60c7 | 98 | |
ojan | 1:6cd6d2760856 | 99 | Vector delta_g = Cross(gyro, g); // Δg = ω × g |
ojan | 1:6cd6d2760856 | 100 | g = 0.9f * (g - delta_g * Freq) + 0.1f * acc.Normalize(); // 相補フィルタ |
ojan | 1:6cd6d2760856 | 101 | g = g.Normalize(); |
ojan | 1:6cd6d2760856 | 102 | |
ojan | 1:6cd6d2760856 | 103 | Vector delta_n = Cross(gyro,n); |
ojan | 1:6cd6d2760856 | 104 | n = 0.9f * (n - delta_n * Freq) + 0.1f * geomag.Normalize(); |
ojan | 1:6cd6d2760856 | 105 | n = n.Normalize(); |
ojan | 1:6cd6d2760856 | 106 | |
ojan | 0:bc6f14fc60c7 | 107 | // LPS25Hによる気圧の取得は10Hz |
ojan | 0:bc6f14fc60c7 | 108 | lps_cnt = (lps_cnt+1)%10; |
ojan | 0:bc6f14fc60c7 | 109 | if(lps_cnt == 0) { |
ojan | 0:bc6f14fc60c7 | 110 | press = (float)lps.pressure() * PRES_LSB_TO_HPA; |
ojan | 0:bc6f14fc60c7 | 111 | } |
ojan | 0:bc6f14fc60c7 | 112 | } |
ojan | 0:bc6f14fc60c7 | 113 | } |