MPUとHMCでうごくかもver
Dependencies: ConfigFile SDFileSystem mbed
Fork of LAURUS_program by
Diff: main.cpp
- Revision:
- 13:df1e8a650185
- Parent:
- 12:0d978eb4d639
- Child:
- 14:f85cb5340cb8
--- a/main.cpp Tue Jun 16 17:04:58 2015 +0000 +++ b/main.cpp Fri Jun 19 01:08:35 2015 +0000 @@ -16,11 +16,7 @@ #define TRUE 1 #define FALSE 0 -#define x 1 -#define y 2 -#define z 3 - -const float dt = 0.1f; // 割り込み周期(s) +const float dt = 0.01f; // 割り込み周期(s) const float ServoMax = 0.0023f; // サーボの最大パルス長(s) const float ServoMin = 0.0006f; // サーボの最小パルス長(s) const float PullMax = 25.0f; // 引っ張れる紐の最大量(mm) @@ -76,9 +72,11 @@ 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) +float yaw = 0.0f; // 本体のヨー角(deg)z軸周り +float pitch = 0.0f; // 本体のピッチ角(deg)y軸周り +float roll = 0.0f; // 本体のロール角(deg)x軸周り + +float vrt_acc = 0.0f; // 鉛直方向の加速度成分(落下検知に使用) /** config.txt ** * #から始めるのはコメント行 @@ -90,27 +88,27 @@ /* ----- Kalman Filter ----- */ // 地磁気ベクトル用 -Vector pri_x1(6); -Matrix pri_P1(6, 6); -Vector post_x1(6); -Matrix post_P1(6, 6); -Matrix F1(6, 6), H1(3, 6); -Matrix R1(6, 6), Q1(3, 3); -Matrix I1(6, 6); -Matrix K1(6, 3); +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); // 重力ベクトル用 // ジャイロのバイアスも同時に推定する -Vector pri_x2(4); -Matrix pri_P2(4, 4); -Vector post_x2(4); -Matrix post_P2(4, 4); -Matrix F2(4, 4), H2(2, 4); -Matrix R2(4, 4), Q2(2, 2); -Matrix I2(4, 4); -Matrix K2(4, 2); -Matrix S2(2, 2), S_inv2(2, 2); +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); /* ----- ------------- ----- */ Timer timer; @@ -166,12 +164,12 @@ raw_g.SetComp(3, 1.0f); // 機体に固定されたベクトルの初期化 - b_f.SetComp(1, 1.0f); + b_f.SetComp(1, 0.0f); b_f.SetComp(2, 0.0f); - b_f.SetComp(3, 0.0f); - b_u.SetComp(1, 0.0f); + b_f.SetComp(3, -1.0f); + b_u.SetComp(1, 1.0f); b_u.SetComp(2, 0.0f); - b_u.SetComp(3, 1.0f); + b_u.SetComp(3, 0.0f); b_l = Cross(b_u, b_f); /* ---------- ↓↓↓ ここからメインループ ↓↓↓ ---------- */ @@ -182,10 +180,10 @@ // 0.1秒おきにセンサーの出力をpcへ出力 myled = 1; // LED is ON - INT_flag = FALSE; // 割り込みによる変数書き換えを阻止 // データ処理 { + INT_flag = FALSE; // 割り込みによる変数書き換えを阻止 gms.read(); // GPSデータ取得 UTC_t = (int)gms.time; @@ -205,18 +203,16 @@ float R_23 = r_l * b_u; // 回転行列の(2,3)成分 float R_33 = r_u * b_u; // 回転行列の(3,3)成分 - yaw = atan2(-R_12, R_11) * RAD_TO_DEG; + yaw = atan2(-R_12, R_11) * RAD_TO_DEG - MAG_DECLINATION; roll = atan2(-R_23, R_33) * RAD_TO_DEG; pitch = asin(R_13) * RAD_TO_DEG; - /* - pc.printf("%.3f, %.3f, %.3f, %.3f\r\n", - gyro.GetComp(1), post_x2.GetComp(3), - gyro.GetComp(2), post_x2.GetComp(4)); - */ - /*pc.printf("%.3f, %.3f, %.3f\r\n", - yaw, roll, pitch); - */ + pc.printf("%.3f, %.3f, %.3f\r\n", + //geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3)); + //yaw, pitch, roll); + post_x2.GetComp(4), + post_x2.GetComp(5), + post_x1.GetComp(7)); if(UTC_t - pre_UTC_t >= 1) { // GPSデータが更新されていたら p.SetComp(1, gms.longitude * DEG_TO_RAD); @@ -227,28 +223,53 @@ 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; + + // データをmicroSDに保存し、XBeeでPCへ送信する + 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, optSensor.read_u16()); + //fprintf(fp, data); + //fflush(fp); + //xbee.printf(data); + + INT_flag = TRUE; // 割り込み許可 } - 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, optSensor.read_u16()); - //fprintf(fp, data); - //fflush(fp); - //xbee.printf(data); - - INT_flag = TRUE; // 割り込み許可 // 制御ルーチン { - pull_L = (pull_L+5)%30; - pull_R = (pull_R+5)%30; + + if(fabs(roll) > 40.0f) { + // スパイラル回避行動 + if(roll > 0) { + pull_L = PullMax; + pull_R = 0; + } else { + pull_L = 0; + pull_R = PullMax; + } + } else { + + } + + //pull_L = (pull_L+5)%PullMax; + //pull_R = (pull_R+5)%PullMax; //pull_L = 0; //pull_R = 30; + + + + // 指定された引っ張り量だけ紐を引っ張る + if(pull_L < 0) pull_L = 0; + else if(pull_L > PullMax) pull_L = PullMax; + if(pull_R < 0) pull_R = 0; + else if(pull_R > PullMax) pull_R = PullMax; + servoL.pulsewidth((ServoMax - ServoMin) * pull_L / PullMax + ServoMin); servoR.pulsewidth((ServoMax - ServoMin) * pull_R / PullMax + ServoMin); @@ -311,119 +332,122 @@ // 重力 { // 誤差共分散行列の値を決める(対角成分のみ) - float alpha_R2 = 0.001f; + float alpha_R2 = 0.002f; float alpha_Q2 = 0.5f; R2 *= alpha_R2; - R2.SetComp(3, 3, 0.003f); - R2.SetComp(4, 4, 0.003f); Q2 *= alpha_Q2; - // 状態方程式のヤコビアンの初期値を代入(時間変化無し) - float f[16] = { - 1.0f, 0.0f, -dt, 0.0f, - 0.0f, 1.0f, 0.0f, -dt, - 0.0f, 0.0f, 1.0f, 0.0f, - 0.0f, 0.0f, 0.0f, 1.0f + // 観測方程式のヤコビアンの値を設定(時間変化無し) + 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 }; - F2.SetComps(f); - - // 観測方程式のヤコビアンの値を設定(時間変化無し) - float h[8] = { - 1.0f, 0.0f, 0.0f, 0.0f, - 0.0f, 1.0f, 0.0f, 0.0f - }; - - H2.SetComps(h); + H2.SetComps(h2); } // 地磁気 { // 誤差共分散行列の値を決める(対角成分のみ) - float alpha_R1 = 500.0f; - float alpha_Q1 = 1000.0f; + float alpha_R1 = 0.002f; + float alpha_Q1 = 0.5f; R1 *= alpha_R1; Q1 *= alpha_Q1; - // 状態方程式のヤコビアンの初期値を代入(時間変化あり) - 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 + // 観測方程式のヤコビアンの値を設定(時間変化無し) + 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 }; - F1.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 - }; - - H1.SetComps(h); + H1.SetComps(h1); } } void KalmanUpdate() { + // 重力 + { - // 入力ベクトル(角速度) - Vector u(4); - u.SetComp(1, raw_gyro.GetComp(1) * dt); - u.SetComp(2, raw_gyro.GetComp(2) * dt); - u.SetComp(3, 0.0f); - u.SetComp(4, 0.0f); + // ヤコビアンの更新 + 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; + //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(); - float det; + 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; - // 観測ベクトル(重力加速度ベクトルから算出した角度) - Vector alpha(2); - alpha.SetComp(1, -atan2(raw_acc.GetComp(2), raw_acc.GetComp(3))); - alpha.SetComp(2, -atan2(raw_acc.GetComp(1), raw_acc.GetComp(3))); - // 事後推定値の更新 - post_x2 = pri_x2 + K2 * (alpha - H2 * pri_x2); + post_x2 = pri_x2 + K2 * (raw_acc - H2 * pri_x2); // 事後誤差分散行列の更新 post_P2 = (I2 - K2 * H2) * pri_P2; } + // 地磁気 { // ヤコビアンの更新 - 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 + 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(f); + F1.SetComps(f1); // 事前推定値の更新 - pri_x1 = F1 * post_x1; + //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(); - float det; + static float det; if((det = S1.Inverse(S_inv1)) >= 0.0f) { pc.printf("E:%.3f\r\n", det); return; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了 @@ -432,6 +456,19 @@ // 事後推定値の更新 post_x1 = pri_x1 + K1 * (raw_geomag - H1 * pri_x1); + // 地磁気ベクトルの大きさに拘束条件を与える + /*{ + Vector gm(3); + gm.SetComp(1, post_x1.GetComp(1)); + gm.SetComp(2, post_x1.GetComp(2)); + gm.SetComp(3, post_x1.GetComp(3)); + + gm = MAG_MAGNITUDE * gm.Normalize(); + + post_x1.SetComp(1, gm.GetComp(1)); + post_x1.SetComp(2, gm.GetComp(2)); + post_x1.SetComp(3, gm.GetComp(3)); + }*/ // 事後誤差分散行列の更新 post_P1 = (I1 - K1 * H1) * pri_P1; } @@ -440,13 +477,13 @@ 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)); + static float mu_y = (p1.GetComp(2) + p2.GetComp(2)) * 0.5f; + static float s_mu_y = sin(mu_y); + static float w = sqrt(1 - GPS_SQ_E * s_mu_y * s_mu_y); + static float m = GPS_A * (1 - GPS_SQ_E) / (w * w * w); + static float n = GPS_A / w; + static float d1 = m * (p1.GetComp(2) - p2.GetComp(2)); + static float d2 = n * cos(mu_y) * (p1.GetComp(1) - p2.GetComp(1)); return sqrt(d1 * d1 + d2 * d2); } @@ -486,6 +523,8 @@ gyro = raw_gyro; press = raw_press; + vrt_acc = raw_acc * raw_g; + } }