MPUとHMCでうごくかもver
Dependencies: ConfigFile SDFileSystem mbed
Fork of LAURUS_program by
main.cpp
- Committer:
- ojan
- Date:
- 2015-06-15
- Revision:
- 9:6d4578dcc1ed
- Parent:
- 8:602865d8fca3
- Child:
- 10:8ee11e412ad7
File content as of revision 9:6d4578dcc1ed:
#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 #define x 1 #define y 2 #define z 3 const float dt = 0.1f; // 割り込み周期(s) const float ServoMax = 0.0023f; // サーボの最大パルス長(s) const float ServoMin = 0.0006f; // サーボの最小パルス長(s) const float PullMax = 25.0f; // 引っ張れる紐の最大量(mm) /********** 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 Log logger(PB_5, PB_4, PB_3, PB_10, PA_9, PA_10, PC_1); // ロガー(microSD、XBee) PwmOut servoL(PB_6), servoR(PC_7); // サーボ用PWM出力 AnalogIn rf(PC_0); // 照度センサ用アナログ入力 AnalogIn servoVcc(PA_0); // バッテリー電圧監視用アナログ入力(サーボ用) AnalogIn logicVcc(PA_1); // バッテリー電圧監視用アナログ入力(ロジック用) Ticker INT_timer; // 割り込みタイマー 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 p(2); // 位置情報(経度, 緯度) Vector pre_p(2); // 過去の位置情報(経度, 緯度) int UTC_t = 0; // UTC時刻 int pre_UTC_t = 0; // 前のUTC時刻 //Vector n(3); // 地磁気ベクトル 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軸) int pull_L = 0; // 左サーボの引っ張り量(mm:0~PullMax) int pull_R = 0; // 右サーボの引っ張り量(mm:0~PullMax) float yaw = 0.0f; // 本体のヨー角(deg) float pitch = 0.0f; // 本体のピッチ角(deg) float roll = 0.0f; // 本体のロール角(deg) /* ----- 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[512] = {}; /********** private functions **********/ void KalmanInit(); // カルマンフィルタ初期化 void KalmanUpdate(); // カルマンフィルタ更新 float distance(Vector p1, Vector p2); // 緯度・経度の差から2点間の距離を算出(m) 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"; // ログのタイトル行 logger.initialize_sdlog(title); // ログファイル初期設定 servoL.period(0.020f); // サーボの信号の周期は20ms servoR.period(0.020f); 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); // 機体に固定されたベクトルの初期化 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); /* ---------- ↓↓↓ ここからメインループ ↓↓↓ ---------- */ while(1) { timer.stop(); timer.reset(); timer.start(); // 0.1秒おきにセンサーの出力をpcへ出力 myled = 1; // LED is ON INT_flag = FALSE; // 割り込みによる変数書き換えを阻止 // データ処理 { gms.read(); // GPSデータ取得 UTC_t = (int)gms.time; // 参照座標系の基底を求める r_u = g; r_f = geomag.GetPerpCompTo(g).Normalize(); r_l = Cross(r_u, r_f); // 回転行列を経由してオイラー角を求める // オイラー角はヨー・ピッチ・ロールの順に回転したものとする // 各オイラー角を求めるのに回転行列の全成分は要らないので、一部だけ計算する。 float R_11 = r_f * b_f; // 回転行列の(1,1)成分 float R_12 = r_f * b_l; // 回転行列の(1,2)成分 float R_13 = r_f * b_u; // 回転行列の(1,3)成分 float R_23 = r_l * b_u; // 回転行列の(2,3)成分 float R_33 = r_u * b_u; // 回転行列の(3,3)成分 yaw = atan2(R_11, -R_12) * RAD_TO_DEG; roll = atan2(R_33, -R_23) * RAD_TO_DEG; pitch = atan2(R_11/cos(yaw), R_13) * RAD_TO_DEG; if(UTC_t - pre_UTC_t >= 1) { // GPSデータが更新されていたら p.SetComp(1, gms.longitude * DEG_TO_RAD); p.SetComp(2, gms.latitude * DEG_TO_RAD); } else { // 更新されていなかったら } pre_p = p; pre_UTC_t = UTC_t; } 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, rf.read_u16()); logger.write(data); INT_flag = TRUE; // 割り込み許可 // 制御ルーチン { //pull_L = (pull_L+5)%30; //pull_R = (pull_R+5)%30; pull_L = 0; pull_R = 30; servoL.pulsewidth((ServoMax - ServoMin) * pull_L / PullMax + ServoMin); servoR.pulsewidth((ServoMax - ServoMin) * pull_R / PullMax + ServoMin); } myled = 0; // LED is OFF // ループはきっかり0.2秒ごと while(timer.read_ms() < 200); pc.printf("time: %.3f[s]\r\n", (float)timer.read_ms()/1000.0f); } /* ---------- ↑↑↑ ここまでメインループ ↑↑↑ ---------- */ } 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; } float distance(Vector p1, Vector p2) { if(p1.GetDim() != p2.GetDim()) return 0.0f; float mu_y = (p1.GetComp(2) + p2.GetComp(2)) * 0.5f; float s_mu_y = sin(mu_y); float w = sqrt(1 - GPS_SQ_E * s_mu_y * s_mu_y); float m = GPS_A * (1 - GPS_SQ_E) / (w * w * w); float n = GPS_A / w; float d1 = m * (p1.GetComp(2) - p2.GetComp(2)); float d2 = n * cos(mu_y) * (p1.GetComp(1) - p2.GetComp(1)); return sqrt(d1 * d1 + d2 * d2); } /* -------------------- 割り込み関数 -------------------- */ 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"); }