MPUとHMCでうごくかもver
Dependencies: ConfigFile SDFileSystem mbed
Fork of LAURUS_program by
main.cpp@0:bc6f14fc60c7, 2015-05-15 (annotated)
- Committer:
- ojan
- Date:
- Fri May 15 17:24:32 2015 +0000
- Revision:
- 0:bc6f14fc60c7
- Child:
- 1:6cd6d2760856
Laurus integrated program (ver.0.0)
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 | 0:bc6f14fc60c7 | 5 | #include "ErrorLogger.h" |
ojan | 0:bc6f14fc60c7 | 6 | #include "Vector.h" |
ojan | 0:bc6f14fc60c7 | 7 | #include "myConstants.h" |
ojan | 0:bc6f14fc60c7 | 8 | |
ojan | 0:bc6f14fc60c7 | 9 | /********** private define **********/ |
ojan | 0:bc6f14fc60c7 | 10 | #define TRUE 1 |
ojan | 0:bc6f14fc60c7 | 11 | #define FALSE 0 |
ojan | 0:bc6f14fc60c7 | 12 | /********** private macro **********/ |
ojan | 0:bc6f14fc60c7 | 13 | /********** private typedef **********/ |
ojan | 0:bc6f14fc60c7 | 14 | /********** private variables **********/ |
ojan | 0:bc6f14fc60c7 | 15 | I2C i2c(I2C_SDA, I2C_SCL); // I2Cポート |
ojan | 0:bc6f14fc60c7 | 16 | MPU6050 mpu(&i2c); // 加速度・角速度センサ |
ojan | 0:bc6f14fc60c7 | 17 | HMC5883L hmc(&i2c); // 地磁気センサ |
ojan | 0:bc6f14fc60c7 | 18 | LPS25H lps(&i2c); // 気圧センサ |
ojan | 0:bc6f14fc60c7 | 19 | Serial pc(SERIAL_TX, SERIAL_RX); // PC通信用シリアルポート |
ojan | 0:bc6f14fc60c7 | 20 | Ticker timer; // 割り込みタイマー |
ojan | 0:bc6f14fc60c7 | 21 | |
ojan | 0:bc6f14fc60c7 | 22 | const float freq = 0.01f; // 割り込み周期(s) |
ojan | 0:bc6f14fc60c7 | 23 | |
ojan | 0:bc6f14fc60c7 | 24 | int lps_cnt = 0; // 気圧センサ読み取りカウント |
ojan | 0:bc6f14fc60c7 | 25 | uint8_t INT_flag = TRUE; // 割り込み可否フラグ |
ojan | 0:bc6f14fc60c7 | 26 | float acc[3]; // 加速度(m/s^2) |
ojan | 0:bc6f14fc60c7 | 27 | float gyro[3]; // 角速度(deg/s) |
ojan | 0:bc6f14fc60c7 | 28 | float geomag[3]; // 地磁気(?) |
ojan | 0:bc6f14fc60c7 | 29 | float press; // 気圧(hPa) |
ojan | 0:bc6f14fc60c7 | 30 | /********** private functions **********/ |
ojan | 0:bc6f14fc60c7 | 31 | void INT_func(); // 割り込み用関数 |
ojan | 0:bc6f14fc60c7 | 32 | /********** main function **********/ |
ojan | 0:bc6f14fc60c7 | 33 | |
ojan | 0:bc6f14fc60c7 | 34 | int main() { |
ojan | 0:bc6f14fc60c7 | 35 | |
ojan | 0:bc6f14fc60c7 | 36 | i2c.frequency(400000); // I2Cの通信速度を400kHzに設定 |
ojan | 0:bc6f14fc60c7 | 37 | |
ojan | 0:bc6f14fc60c7 | 38 | if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!"); // mpu6050初期化 |
ojan | 0:bc6f14fc60c7 | 39 | if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!"); // hmc5883l初期化 |
ojan | 0:bc6f14fc60c7 | 40 | |
ojan | 0:bc6f14fc60c7 | 41 | timer.attach(&INT_func, freq); // 割り込み有効化 |
ojan | 0:bc6f14fc60c7 | 42 | |
ojan | 0:bc6f14fc60c7 | 43 | while(1) { |
ojan | 0:bc6f14fc60c7 | 44 | |
ojan | 0:bc6f14fc60c7 | 45 | INT_flag = FALSE; // 割り込みによる変数書き換えを阻止 |
ojan | 0:bc6f14fc60c7 | 46 | |
ojan | 0:bc6f14fc60c7 | 47 | // センサ類の全出力値をPCに送信 |
ojan | 0:bc6f14fc60c7 | 48 | printf("%f\t", acc[0]); |
ojan | 0:bc6f14fc60c7 | 49 | printf("%f\t", acc[1]); |
ojan | 0:bc6f14fc60c7 | 50 | printf("%f\t", acc[2]); |
ojan | 0:bc6f14fc60c7 | 51 | printf("%f\t", gyro[0]); |
ojan | 0:bc6f14fc60c7 | 52 | printf("%f\t", gyro[1]); |
ojan | 0:bc6f14fc60c7 | 53 | printf("%f\t", gyro[2]); |
ojan | 0:bc6f14fc60c7 | 54 | printf("%f\t", geomag[0]); |
ojan | 0:bc6f14fc60c7 | 55 | printf("%f\t", geomag[1]); |
ojan | 0:bc6f14fc60c7 | 56 | printf("%f\t", geomag[2]); |
ojan | 0:bc6f14fc60c7 | 57 | printf("%f\r\n", press); |
ojan | 0:bc6f14fc60c7 | 58 | |
ojan | 0:bc6f14fc60c7 | 59 | INT_flag = TRUE; // 割り込み許可 |
ojan | 0:bc6f14fc60c7 | 60 | |
ojan | 0:bc6f14fc60c7 | 61 | } |
ojan | 0:bc6f14fc60c7 | 62 | } |
ojan | 0:bc6f14fc60c7 | 63 | |
ojan | 0:bc6f14fc60c7 | 64 | void INT_func() { |
ojan | 0:bc6f14fc60c7 | 65 | if(INT_flag == FALSE) { |
ojan | 0:bc6f14fc60c7 | 66 | |
ojan | 0:bc6f14fc60c7 | 67 | mpu.read(); |
ojan | 0:bc6f14fc60c7 | 68 | hmc.read(); |
ojan | 0:bc6f14fc60c7 | 69 | |
ojan | 0:bc6f14fc60c7 | 70 | for(int i=0; i<3; i++) { |
ojan | 0:bc6f14fc60c7 | 71 | acc[i] = (float)mpu.data.value.acc[i] * ACC_LSB_TO_G * G_TO_MPSS; |
ojan | 0:bc6f14fc60c7 | 72 | gyro[i] = mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG; |
ojan | 0:bc6f14fc60c7 | 73 | geomag[i] = hmc.data.value[i]; |
ojan | 0:bc6f14fc60c7 | 74 | } |
ojan | 0:bc6f14fc60c7 | 75 | |
ojan | 0:bc6f14fc60c7 | 76 | // LPS25Hによる気圧の取得は10Hz |
ojan | 0:bc6f14fc60c7 | 77 | lps_cnt = (lps_cnt+1)%10; |
ojan | 0:bc6f14fc60c7 | 78 | if(lps_cnt == 0) { |
ojan | 0:bc6f14fc60c7 | 79 | press = (float)lps.pressure() * PRES_LSB_TO_HPA; |
ojan | 0:bc6f14fc60c7 | 80 | } |
ojan | 0:bc6f14fc60c7 | 81 | } |
ojan | 0:bc6f14fc60c7 | 82 | } |