MPUとHMCでうごくかもver
Dependencies: ConfigFile SDFileSystem mbed
Fork of LAURUS_program by
Diff: main.cpp
- Revision:
- 0:bc6f14fc60c7
- Child:
- 1:6cd6d2760856
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri May 15 17:24:32 2015 +0000 @@ -0,0 +1,82 @@ +#include "mbed.h" +#include "MPU6050.h" +#include "HMC5883L.h" +#include "LPS25H.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 pc(SERIAL_TX, SERIAL_RX); // PC通信用シリアルポート +Ticker timer; // 割り込みタイマー + +const float freq = 0.01f; // 割り込み周期(s) + +int lps_cnt = 0; // 気圧センサ読み取りカウント +uint8_t INT_flag = TRUE; // 割り込み可否フラグ +float acc[3]; // 加速度(m/s^2) +float gyro[3]; // 角速度(deg/s) +float geomag[3]; // 地磁気(?) +float press; // 気圧(hPa) +/********** 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); // 割り込み有効化 + + while(1) { + + INT_flag = FALSE; // 割り込みによる変数書き換えを阻止 + + // センサ類の全出力値をPCに送信 + printf("%f\t", acc[0]); + printf("%f\t", acc[1]); + printf("%f\t", acc[2]); + printf("%f\t", gyro[0]); + printf("%f\t", gyro[1]); + printf("%f\t", gyro[2]); + printf("%f\t", geomag[0]); + printf("%f\t", geomag[1]); + printf("%f\t", geomag[2]); + 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[i] = (float)mpu.data.value.acc[i] * ACC_LSB_TO_G * G_TO_MPSS; + gyro[i] = mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG; + geomag[i] = hmc.data.value[i]; + } + + // LPS25Hによる気圧の取得は10Hz + lps_cnt = (lps_cnt+1)%10; + if(lps_cnt == 0) { + press = (float)lps.pressure() * PRES_LSB_TO_HPA; + } + } +} \ No newline at end of file