MPUとHMCでうごくかもver
Dependencies: ConfigFile SDFileSystem mbed
Fork of LAURUS_program by
Diff: main.cpp
- Revision:
- 9:6d4578dcc1ed
- Parent:
- 8:602865d8fca3
- Child:
- 10:8ee11e412ad7
--- a/main.cpp Fri Jun 12 15:28:05 2015 +0000 +++ b/main.cpp Mon Jun 15 10:40:18 2015 +0000 @@ -14,9 +14,14 @@ #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 **********/ @@ -50,10 +55,26 @@ 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); // 地磁気ベクトル -int pull_L = 0; // 左サーボの引っ張り量(mm:0~30) -int pull_R = 0; // 右サーボの引っ張り量(mm:0~30) +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); @@ -72,9 +93,10 @@ char data[512] = {}; /********** private functions **********/ -void KalmanInit(); // カルマンフィルタ初期化 -void KalmanUpdate(); // カルマンフィルタ更新 -void INT_func(); // 割り込み用関数 +void KalmanInit(); // カルマンフィルタ初期化 +void KalmanUpdate(); // カルマンフィルタ更新 +float distance(Vector p1, Vector p2); // 緯度・経度の差から2点間の距離を算出(m) +void INT_func(); // 割り込み用関数 void toString(Matrix& m); void toString(Vector& v); @@ -102,6 +124,15 @@ 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(); @@ -109,14 +140,44 @@ timer.start(); // 0.1秒おきにセンサーの出力をpcへ出力 myled = 1; // LED is ON - wait(0.05f); // 50 ms - myled = 0; // LED is OFF - - pull_L = (pull_L+5)%30; - pull_R = (pull_R+5)%30; 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; @@ -129,20 +190,23 @@ INT_flag = TRUE; // 割り込み許可 - pc.printf("time: %.3f[s]\r\n", (float)timer.read_ms()/1000.0f); - // 制御ルーチン - /*{ - servoL.pulsewidth((ServoMax-ServoMin) * pull_L / 30.0f + ServoMin); - servoR.pulsewidth((ServoMax-ServoMin) * pull_R / 30.0f + ServoMin); + { + //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); - // ループはきっかり1秒ごと - while(timer.read_ms() < 1000); + pc.printf("time: %.3f[s]\r\n", (float)timer.read_ms()/1000.0f); } @@ -212,6 +276,22 @@ 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(); @@ -248,6 +328,8 @@ } } +/* -------------------- デバッグ用関数 -------------------- */ + void toString(Matrix& m) { for(int i=0; i<m.GetRow(); i++) {