hiroya taura / Mbed 2 deprecated LAURUS_program

MPU6050のサンプルプログラム２

Dependencies:   ConfigFile SDFileSystem mbed

Fork of LAURUS_program by LAURUS

main.cpp@1:6cd6d2760856, 2015-05-24 (annotated)

Committer:
ojan
Date:
Sun May 24 17:32:47 2015 +0000
Revision:
1:6cd6d2760856
Parent:
0:bc6f14fc60c7
Child:
2:d2b60a1d0cd9
Send all sensor's data to PC

Who changed what in which revision?

UserRevisionLine numberNew 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 0:bc6f14fc60c7 59 while(1) {
ojan 0:bc6f14fc60c7 60
ojan 1:6cd6d2760856 61 // 1秒おきにセンサーの出力をpcへ出力
ojan 1:6cd6d2760856 62 wait(1.0f);
ojan 1:6cd6d2760856 63
ojan 0:bc6f14fc60c7 64 INT_flag = FALSE; // 割り込みによる変数書き換えを阻止
ojan 0:bc6f14fc60c7 65
ojan 0:bc6f14fc60c7 66 // センサ類の全出力値をPCに送信
ojan 1:6cd6d2760856 67 pc.printf("%f,", acc.GetComp(1));
ojan 1:6cd6d2760856 68 pc.printf("%f,", acc.GetComp(2));
ojan 1:6cd6d2760856 69 pc.printf("%f,", acc.GetComp(3));
ojan 1:6cd6d2760856 70 pc.printf("%f,", gyro.GetComp(1));
ojan 1:6cd6d2760856 71 pc.printf("%f,", gyro.GetComp(2));
ojan 1:6cd6d2760856 72 pc.printf("%f,", gyro.GetComp(3));
ojan 1:6cd6d2760856 73 pc.printf("%f,", geomag.GetComp(1));
ojan 1:6cd6d2760856 74 pc.printf("%f,", geomag.GetComp(2));
ojan 1:6cd6d2760856 75 pc.printf("%f,", geomag.GetComp(3));
ojan 1:6cd6d2760856 76 pc.printf("%f\r\n", press);
ojan 0:bc6f14fc60c7 77
ojan 0:bc6f14fc60c7 78 INT_flag = TRUE; // 割り込み許可
ojan 0:bc6f14fc60c7 79
ojan 0:bc6f14fc60c7 80 }
ojan 0:bc6f14fc60c7 81 }
ojan 0:bc6f14fc60c7 82
ojan 0:bc6f14fc60c7 83 void INT_func() {
ojan 0:bc6f14fc60c7 84 if(INT_flag == FALSE) {
ojan 0:bc6f14fc60c7 85
ojan 1:6cd6d2760856 86 // センサーの値を更新
ojan 0:bc6f14fc60c7 87 mpu.read();
ojan 0:bc6f14fc60c7 88 hmc.read();
ojan 0:bc6f14fc60c7 89
ojan 0:bc6f14fc60c7 90 for(int i=0; i<3; i++) {
ojan 1:6cd6d2760856 91 acc.SetComp(i+1, (float)mpu.data.value.acc[i] * ACC_LSB_TO_G);
ojan 1:6cd6d2760856 92 gyro.SetComp(i+1, (float)mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD);
ojan 1:6cd6d2760856 93 geomag.SetComp(i+1, (float)hmc.data.value[i] * MAG_LSB_TO_GAUSS);
ojan 0:bc6f14fc60c7 94 }
ojan 0:bc6f14fc60c7 95
ojan 1:6cd6d2760856 96 Vector delta_g = Cross(gyro, g); // Δg = ω × g
ojan 1:6cd6d2760856 97 g = 0.9f * (g - delta_g * Freq) + 0.1f * acc.Normalize(); // 相補フィルタ
ojan 1:6cd6d2760856 98 g = g.Normalize();
ojan 1:6cd6d2760856 99
ojan 1:6cd6d2760856 100 Vector delta_n = Cross(gyro,n);
ojan 1:6cd6d2760856 101 n = 0.9f * (n - delta_n * Freq) + 0.1f * geomag.Normalize();
ojan 1:6cd6d2760856 102 n = n.Normalize();
ojan 1:6cd6d2760856 103
ojan 0:bc6f14fc60c7 104 // LPS25Hによる気圧の取得は10Hz
ojan 0:bc6f14fc60c7 105 lps_cnt = (lps_cnt+1)%10;
ojan 0:bc6f14fc60c7 106 if(lps_cnt == 0) {
ojan 0:bc6f14fc60c7 107 press = (float)lps.pressure() * PRES_LSB_TO_HPA;
ojan 0:bc6f14fc60c7 108 }
ojan 0:bc6f14fc60c7 109 }
ojan 0:bc6f14fc60c7 110 }