MPUとHMCでうごくかもver
Dependencies: ConfigFile SDFileSystem mbed
Fork of LAURUS_program by
main.cpp
- Committer:
- YusukeWakuta
- Date:
- 2015-12-26
- Revision:
- 43:3a37e39b234c
- Parent:
- 42:7428acb9b14d
File content as of revision 43:3a37e39b234c:
#include "mbed.h" #include "MPU6050.h" #include "HMC5883L.h" #include "LPS25H.h" #include "GMS6_CR6.h" #include "Vector.h" #include "Matrix.h" #include "Vector_Matrix_operator.h" #include "myConstants.h" #include "SDFileSystem.h" #include "BufferedSerial.h" #include "ConfigFile.h" const float dt = 0.01f; // 割り込み周期(s) enum Direction {LEFT, RIGHT, NEUTRAL}; /****************************** private macro ******************************/ /****************************** private typedef ******************************/ /****************************** private variables ******************************/ DigitalOut myled(LED1); // デバッグ用LEDのためのデジタル出力 I2C i2c_mpu(p28,p27); // i2c_mpuポート I2C i2c_hmc(p9,p10); MPU6050 mpu(&i2c_mpu); // 加速度・角速度センサ HMC5883L hmc(&i2c_hmc); // 地磁気センサ //LPS25H lps(&i2c_mpu); // 気圧センサ Serial pc(USBTX,USBRX); // PC通信用シリアルポート FILE * fp; // ログファイルのポインタ ConfigFile cfg; // ConfigFile Ticker ticker; // 割り込みタイマー Timer timer; // 時間計測用タイマー Direction dir = NEUTRAL; // 旋回方向 int i; uint8_t test_val = 100; volatile int lps_cnt = 0; // 気圧センサ読み取りカウント volatile bool INT_flag = true; // 割り込み可否フラグ /* こちらの変数群はメインループでは参照しない */ Vector raw_acc(3); // 加速度(m/s^2) 生 Vector raw_gyro(3); // 角速度(deg/s) 生 Vector raw_geomag(3); // 地磁気(?) 生 float raw_press; // 気圧(hPa) 生 float raw_temp; // 温度(℃) 生 /* メインループ内ではこちらを参照する */ Vector acc(3); // 加速度(m/s^2) Vector gyro(3); // 角速度(deg/s) Vector geomag(3); // 地磁気(?) float p0; // 気圧の初期値 float press; // 気圧(hPa) float temp; // 温度(℃) float height; // 高さ(m) Vector raw_g(3); // 重力ベクトル 生 Vector g(3); // 重力ベクトル Vector delta_g(3); // Δg int UTC_t = 0; // UTC時刻 int pre_UTC_t = 0; // 前のUTC時刻 int ss = 0; // 時刻の秒数の小数部分 int boolcheck; //check INT_frag ,true or false Vector b_f(3); // 機体座標に固定された、機体前方向きのベクトル(x軸) Vector b_u(3); // 機体座標に固定された、機体上方向きのベクトル(z軸) Vector b_l(3); // 機体座標に固定された、機体左方向きのベクトル(y軸) Vector r_f(3); // 世界座標に固定された、北向きのベクトル(X軸) Vector r_u(3); // 世界座標に固定された、上向きのベクトル(Z軸) Vector r_l(3); // 世界座標に固定された、西向きのベクトル(Y軸) float yaw = 0.0f; // 本体のヨー角(deg)z軸周り float pitch = 0.0f; // 本体のピッチ角(deg)y軸周り float roll = 0.0f; // 本体のロール角(deg)x軸周り float vrt_acc = 0.0f; // 鉛直方向の加速度成分(落下検知に使用) char data[512] = {}; // 送信データ用配 int loopTime = 0; // 1ループに掛かる時間(デバッグ用 float sv = 0.0f; // サーボ電源電圧 float lv = 0.0f; // ロジック電源電圧 #ifdef RULE3_1 float beta = 0.0f; // β変数 (制御則3_1で使用 #endif /** config.txt ** * #から始めるのはコメント行 * #イコールの前後に空白を入れない * target_x=111.222 * target_y=33.444 */ float target_x, target_y; /* ---------- Kalman Filter ---------- */ // 地磁気ベクトル用 // ジャイロのz軸周りのバイアスも推定 Vector pri_x1(7); Matrix pri_P1(7, 7); Vector post_x1(7); Matrix post_P1(7, 7); Matrix F1(7, 7), H1(3, 7); Matrix R1(7, 7), Q1(3, 3); Matrix I1(7, 7); Matrix K1(7, 3); Matrix S1(3, 3), S_inv1(3, 3); // 重力ベクトル用 // ジャイロのx軸、y軸周りのバイアスも同時に推定 Vector pri_x2(5); Matrix pri_P2(5, 5); Vector post_x2(5); Matrix post_P2(5, 5); Matrix F2(5, 5), H2(3, 5); Matrix R2(5, 5), Q2(3, 3); Matrix I2(5, 5); Matrix K2(5, 3); Matrix S2(3, 3), S_inv2(3, 3); /* ---------- ------------- ---------- */ /****************************** private functions ******************************/ void SensorsInit(); // 各種センサーの初期化 void KalmanInit(); // カルマンフィルタ初期化 void DataProcessing(); // データ処理関数 void KalmanUpdate(); // カルマンフィルタ更新 void DataUpdate(); // データ取得&更新関数 /** 小さい値を返す関数 * @param a: 1つ目の入力値 * @param b: 2つ目の入力値 * * @return a,bのうち、小さい方の値 */ inline float min(float a, float b) { return ((a > b) ? b : a); } void toString(Vector& v) { for(int i=0; i<v.GetDim(); i++) { // pc.printf("%.6f\t", v.GetComp(i+1)); } // pc.printf("\r\n"); } /****************************** main function ******************************/ int main() { //重力ベクトルの初期化 raw_g.SetComp(1, 0.0f); raw_g.SetComp(2, 0.0f); raw_g.SetComp(3, 1.0f); //↓mpuのスリープモードを解除するコードを含んだ関数(SensorsInit())が宣言だけされて実装されてなかったので追加(w) SensorsInit(); //↓i2c_mpu.startたしたけど変化なし i2c_mpu.start(); i2c_hmc.start(); i2c_mpu.frequency(400000); i2c_hmc.frequency(400000); // i2c_hmcの通信速度を400kHzに設定 //カルマンフィルタ初期化 KalmanInit(); ticker.attach(&DataUpdate, dt); // 割り込み有効化(Freq = 0.01fなので、10msおきの割り込み) /* ------------------------------ ↓↓↓ ここからメインループ ↓↓↓ ------------------------------ */ while(1) { timer.stop(); timer.reset(); timer.start(); myled = 1; // LED is ON INT_flag = true; // 割り込みによる変数書き換えを阻止 /******************************* データ処理 *******************************/ // DataProcessing(); /*//check INT_flag true or false(w)================= if(INT_flag) boolcheck = 1; else if(!INT_flag) boolcheck = 0; pc.printf("%d",boolcheck); //==================================================== */ wait_ms(150); } /* ------------------------------ ↑↑↑ ここまでメインループ ↑↑↑ ------------------------------ */ } /** 各種センサーの初期化を行う関数 * */ void SensorsInit() { if(!mpu.init()) error("mpu6050 Initialize Error !!"); // mpu6050初期化 if(!hmc.init()) error("hmc5883l Initialize Error !!"); // hmc5883l初期化 // 機体に固定されたベクトルの初期化 b_f.SetComp(1, 0.0f); b_f.SetComp(2, 0.0f); b_f.SetComp(3, -1.0f); b_u.SetComp(1, 1.0f); b_u.SetComp(2, 0.0f); b_u.SetComp(3, 0.0f); b_l = Cross(b_u, b_f); // 目標座標をベクトルに代入 // target_p.SetComp(1, target_x * DEG_TO_RAD); //target_p.SetComp(2, target_y * DEG_TO_RAD); } void KalmanInit() { // 重力 { // 誤差共分散行列の値を決める(対角成分のみ) float alpha_R2 = 0.002f; float alpha_Q2 = 0.5f; R2 *= alpha_R2; Q2 *= alpha_Q2; // 観測方程式のヤコビアンの値を設定(時間変化無し) float h2[15] = { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f }; H2.SetComps(h2); } // 地磁気 { // 誤差共分散行列の値を決める(対角成分のみ) float alpha_R1 = 0.002f; float alpha_Q1 = 0.5f; R1 *= alpha_R1; Q1 *= alpha_Q1; // 観測方程式のヤコビアンの値を設定(時間変化無し) float h1[21] = { 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f }; H1.SetComps(h1); } } /** カルマンフィルタの更新を行う関数 * */ void KalmanUpdate() { // 重力 { // ヤコビアンの更新 float f2[25] = { 1.0f, (raw_gyro.GetComp(3) - post_x1.GetComp(7))*dt, -(raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, 0.0f, post_x2.GetComp(3)*dt, -(raw_gyro.GetComp(3) - post_x1.GetComp(7))*dt, 1.0f, (raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, -post_x2.GetComp(3)*dt, 0.0f, (raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, -(raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, 1.0f, post_x2.GetComp(2)*dt, -post_x2.GetComp(1)*dt, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f }; F2.SetComps(f2); // 事前推定値の更新 pri_x2 = F2 * post_x2; float pri_x2_vals[5] = { post_x2.GetComp(1) + post_x2.GetComp(2) * (raw_gyro.GetComp(3) - post_x1.GetComp(7)) * dt - post_x2.GetComp(3) * (raw_gyro.GetComp(2) - post_x2.GetComp(5)) * dt, post_x2.GetComp(2) + post_x2.GetComp(3) * (raw_gyro.GetComp(1) - post_x2.GetComp(4)) * dt - post_x2.GetComp(1) * (raw_gyro.GetComp(3) - post_x1.GetComp(7)) * dt, post_x2.GetComp(3) + post_x2.GetComp(1) * (raw_gyro.GetComp(2) - post_x2.GetComp(5)) * dt - post_x2.GetComp(2) * (raw_gyro.GetComp(1) - post_x2.GetComp(4)) * dt, post_x2.GetComp(4), post_x2.GetComp(5) }; pri_x2.SetComps(pri_x2_vals); // 事前誤差分散行列の更新 pri_P2 = F2 * post_P2 * F2.Transpose() + R2; // カルマンゲインの計算 S2 = Q2 + H2 * pri_P2 * H2.Transpose(); static float det; if((det = S2.Inverse(S_inv2)) >= 0.0f) { pc.printf("E:%.3f\r\n", det); return; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了 } K2 = pri_P2 * H2.Transpose() * S_inv2; // 事後推定値の更新 post_x2 = pri_x2 + K2 * (raw_acc - H2 * pri_x2); // 事後誤差分散行列の更新 post_P2 = (I2 - K2 * H2) * pri_P2; } // 地磁気 { // ヤコビアンの更新 float f1[49] = { 1.0f, (raw_gyro.GetComp(3) - post_x1.GetComp(7))*dt, -(raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, 0.0f, 0.0f, 0.0f, -post_x1.GetComp(2) * dt, -(raw_gyro.GetComp(3) - post_x1.GetComp(7))*dt, 1.0f, (raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, 0.0f, 0.0f, 0.0f, post_x1.GetComp(1) * dt, (raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, -(raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, 1.0f, 0.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, 0.0f, 1.0f, 0.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, 0.0f, 1.0f }; F1.SetComps(f1); // 事前推定値の更新 //pri_x1 = F1 * post_x1; float pri_x1_vals[7] = { post_x1.GetComp(1) + post_x1.GetComp(2) * (raw_gyro.GetComp(3) - post_x1.GetComp(7)) * dt - post_x1.GetComp(3) * (raw_gyro.GetComp(2) - post_x2.GetComp(5)) * dt, post_x1.GetComp(2) + post_x1.GetComp(3) * (raw_gyro.GetComp(1) - post_x2.GetComp(4)) * dt - post_x1.GetComp(1) * (raw_gyro.GetComp(3) - post_x1.GetComp(7)) * dt, post_x1.GetComp(3) + post_x1.GetComp(1) * (raw_gyro.GetComp(2) - post_x2.GetComp(5)) * dt - post_x1.GetComp(2) * (raw_gyro.GetComp(1) - post_x2.GetComp(4)) * dt, post_x1.GetComp(4), post_x1.GetComp(5), post_x1.GetComp(6), post_x1.GetComp(7) }; pri_x1.SetComps(pri_x1_vals); // 事前誤差分散行列の更新 pri_P1 = F1 * post_P1 * F1.Transpose() + R1; // カルマンゲインの計算 S1 = Q1 + H1 * pri_P1 * H1.Transpose(); static float det; if((det = S1.Inverse(S_inv1)) >= 0.0f) { pc.printf("E:%.3f\r\n", det); return; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了 } K1 = pri_P1 * H1.Transpose() * S_inv1; // 事後推定値の更新 post_x1 = pri_x1 + K1 * (raw_geomag - H1 * pri_x1); // 事後誤差分散行列の更新 post_P1 = (I1 - K1 * H1) * pri_P1; } } /* ------------------------------ 割り込み関数 ------------------------------ */ /** データ取得&更新関数 * */ void DataUpdate() { // センサーの値を更新 mpu.read(); hmc.read(); /*//↓i2c_mpuの通信に成功しているかどうかを確認(w) //=========================================================== if(mpu.checker_get() == 0){ pc.printf("SUCCESS!!\n\r"); } for(i = 0;i < 13;i++){ pc.printf("%u\n\r",mpu.data.reg[i]); } //============================================================ */ 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); //pc.printf("raw_acc/%d:%10f raw_gyro/%d:%10f\n\r",i,raw_acc.GetComp(i), i,raw_gyro.GetComp(i)); } raw_g.SetComp(1, 0.0f); raw_g.SetComp(2, 0.0f); raw_g.SetComp(3, 1.0f); delta_g = Cross(raw_gyro, raw_g); // Δg = ω × g //for(int i = 0;i < 3;i++){ // pc.printf("delta_g%d : %7f\n\r",i,delta_g.GetComp(i)); // } //for(int i = 0; i < delta_g.Getdim(); i++) { // pc.printf("delta_g%d:%f\n\r",i,delta_g.GetComp(i)); // } raw_g = 0.9f * (raw_g - delta_g * dt) + 0.1f * raw_acc.Normalize(); // 相補フィルタ /* for(int i = 1; i <= raw_g.Getdim(); i++) { pc.printf("%f",raw_g.GetComp(i)); } */ raw_g = raw_g.Normalize(); //========================================= KalmanUpdate(); /*//check INT_flag true or false(w)================= if(INT_flag) boolcheck = 1; else if(!INT_flag) boolcheck = 0; pc.printf("%d",boolcheck); //====================================================*/ if(1) { g = raw_g; for(int i=0; i<3; i++) { geomag.SetComp(i+1, post_x1.GetComp(i+1)); } acc = raw_acc; gyro = raw_gyro; press = raw_press; temp = raw_temp; vrt_acc = raw_acc * raw_g; } static float R_11; // 回転行列(1,1)成分 static float R_12; // 回転行列(1,2)成分 static float r_cos; // ロール角のcos値 static float r_sin; // ロール角のsin値 static float p_cos; // ピッチ角のcos値 static float p_sin; // ピッチ角のsin値 // 参照座標系の基底を求める r_u = g; r_l = Cross(r_u, r_f); r_f = geomag.GetPerpCompTo(g).Normalize(); // 回転行列を経由してオイラー角を求める // オイラー角はヨー・ピッチ・ロールの順に回転したものとする // 各オイラー角を求めるのに回転行列の全成分は要らないので、一部だけ計算する。 R_11 = r_f * b_f; // 回転行列の(1,1)成分 R_12 = r_f * b_l; // 回転行列の(1,2)成分 yaw = atan2(-R_12, R_11) * RAD_TO_DEG + MAG_DECLINATION; r_cos = g.GetPerpCompTo(b_f).Normalize() * b_u; r_sin = Cross(g.GetPerpCompTo(b_f).Normalize(), b_u) * b_f; if(r_sin > 0.0f) { roll = acos(r_cos) * RAD_TO_DEG; } else { roll = -acos(r_cos) * RAD_TO_DEG; } p_cos = g.GetPerpCompTo(b_l).Normalize() * b_u; p_sin = Cross(g.GetPerpCompTo(b_l).Normalize(), b_u) * b_l; if(p_sin > 0.0f) { pitch = acos(p_cos) * RAD_TO_DEG; } else { pitch = -acos(p_cos) * RAD_TO_DEG; } if(yaw > 180.0f) yaw -= 360.0f; // ヨー角を[-π, π]に補正 else if(yaw < -180.0f) yaw += 360.0f; // ヨー角を[-π, π]に補正 //下のroll,pitch,yawを表示させるprintfでnanと出るので、変数の中に何も値が代入されていない可能性を調べた。→値は入ってるらしい。(w)======================== pc.printf("pitch:%7f roll:%7f yaw:%7f\n\r",pitch,roll,yaw); }