MPUとHMCでうごくかもver
Dependencies: ConfigFile SDFileSystem mbed
Fork of LAURUS_program by
main.cpp
- Committer:
- ojan
- Date:
- 2015-06-11
- Revision:
- 5:182f6356bce1
- Parent:
- 4:45dc5590abc0
- Child:
- 6:2b68f85a984a
File content as of revision 5:182f6356bce1:
#include "mbed.h" #include "MPU6050.h" #include "HMC5883L.h" #include "LPS25H.h" #include "GMS6_CR6.h" #include "ErrorLogger.h" #include "Vector.h" #include "Matrix.h" #include "Vector_Matrix_operator.h" #include "myConstants.h" #include "Log.h" /********** private define **********/ #define TRUE 1 #define FALSE 0 /********** private macro **********/ /********** private typedef **********/ /********** private variables **********/ DigitalOut myled(LED1); // デバッグ用LEDのためのデジタル出力 I2C i2c(PB_9, PB_8); // I2Cポート MPU6050 mpu(&i2c); // 加速度・角速度センサ HMC5883L hmc(&i2c); // 地磁気センサ LPS25H lps(&i2c); // 気圧センサ Serial gps(PA_11, PA_12); // GPS通信用シリアルポート Serial pc(SERIAL_TX, SERIAL_RX); // PC通信用シリアルポート GMS6_CR6 gms(&gps, &pc); // GPS Ticker INT_timer; // 割り込みタイマー Log logger(PA_9, PA_10, PC_1, PB_5, PB_4, PB_3, PB_10); // ロガー(microSD、XBee) PwmOut servoL(PC_7), servoR(PB_6); AnalogIn rf(PC_0); AnalogIn servoVcc(PA_0); AnalogIn logicVcc(PA_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 raw_g(3); // 重力ベクトル 生 Vector g(3); // 重力ベクトル 生 //Vector n(3); // 地磁気ベクトル /* ----- Kalman Filter ----- */ Vector pri_x(6); Matrix pri_P(6, 6); Vector post_x(6); Matrix post_P(6, 6); Matrix F(6, 6), H(3, 6); Matrix R(6, 6), Q(3, 3); Matrix I(6, 6); Matrix K(6, 3); Matrix S(3, 3), inv(3, 3); /* ----- ------------- ----- */ Timer timer; char data[1024] = {}; /********** private functions **********/ void KalmanInit(); // カルマンフィルタ初期化 void KalmanUpdate(); // カルマンフィルタ更新 void INT_func(); // 割り込み用関数 void toString(Matrix& m); void toString(Vector& v); /********** 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初期化 char* title = "log data\r\n"; // ログのタイトル行 if(!logger.initialize_sdlog(title)) return 0; // ログファイル初期設定 KalmanInit(); INT_timer.attach(&INT_func, dt); // 割り込み有効化(Freq = 0.01fなので、10msおきの割り込み) //重力ベクトルの初期化 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へ出力 myled = 1; // LED is ON wait(0.05f); // 50 ms myled = 0; // LED is OFF INT_flag = FALSE; // 割り込みによる変数書き換えを阻止 float sv = (float)servoVcc.read_u16() * ADC_LSB_TO_V * 2.0f; float lv = (float)logicVcc.read_u16() * ADC_LSB_TO_V * 2.0f; sprintf(data, "%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%d\r\n", g.GetComp(1), g.GetComp(2), g.GetComp(3), geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3), press, gms.longitude, gms.latitude, sv, lv, gms.Ns); 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); } /* ---------- ↑↑↑ ここまでメインループ ↑↑↑ ---------- */ } void KalmanInit() { // 誤差共分散行列の値を決める(対角成分のみ) float alpha_R = 60.0f; float alpha_Q = 100.0f; R *= alpha_R; Q *= alpha_Q; // 状態方程式のヤコビアンの初期値を代入(時間変化あり) float f[36] = { 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 }; F.SetComps(f); // 観測方程式のヤコビアンの値を設定(時間変化無し) float h[18] = { 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f }; H.SetComps(h); } void KalmanUpdate() { // ヤコビアンの更新 float f[36] = { 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 }; F.SetComps(f); // 事前推定値の更新 pri_x = F * post_x; // 事前誤差分散行列の更新 pri_P = F * post_P * F.Transpose() + R; // カルマンゲインの計算 S = Q + H * pri_P * H.Transpose(); float det; if((det = S.Inverse(inv)) >= 0.0f) { pc.printf("E:%.3f\r\n", det); return; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了 } K = pri_P * H.Transpose() * inv; // 事後推定値の更新 post_x = pri_x + K * (raw_geomag - H * pri_x); // 事後誤差分散行列の更新 post_P = (I - K * H) * pri_P; } void INT_func() { // センサーの値を更新 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); } 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(); // 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()); 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; } } void toString(Matrix& m) { for(int i=0; i<m.GetRow(); i++) { for(int j=0; j<m.GetCol(); j++) { pc.printf("%.6f\t", m.GetComp(i+1, j+1)); } pc.printf("\r\n"); } } void toString(Vector& v) { for(int i=0; i<v.GetDim(); i++) { pc.printf("%.6f\t", v.GetComp(i+1)); } pc.printf("\r\n"); }