albatross
/
LAURUS_program_copy
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: ConfigFile SDFileSystem mbed
Fork of
LAURUS_program
by 
hiroya taura
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "MPU6050.h" 00003 #include "HMC5883L.h" 00004 #include "LPS25H.h" 00005 #include "GMS6_CR6.h" 00006 #include "Vector.h" 00007 #include "Matrix.h" 00008 #include "Vector_Matrix_operator.h" 00009 #include "myConstants.h" 00010 #include "SDFileSystem.h" 00011 #include "BufferedSerial.h" 00012 #include "ConfigFile.h" 00013 00014 const float dt = 0.01f; // 割り込み周期(s) 00015 00016 enum Direction {LEFT, RIGHT, NEUTRAL}; 00017 00018 /****************************** private macro ******************************/ 00019 /****************************** private typedef ******************************/ 00020 /****************************** private variables ******************************/ 00021 DigitalOut myled(LED1); // デバッグ用LEDのためのデジタル出力 00022 I2C i2c_mpu(p28,p27); // i2c_mpuポート 00023 I2C i2c_hmc(p9,p10); 00024 MPU6050 mpu(&i2c_mpu); // 加速度・角速度センサ 00025 HMC5883L hmc(&i2c_hmc); // 地磁気センサ 00026 //LPS25H lps(&i2c_mpu); // 気圧センサ 00027 Serial pc(USBTX,USBRX); // PC通信用シリアルポート 00028 FILE * fp; // ログファイルのポインタ 00029 ConfigFile cfg; // ConfigFile 00030 Ticker ticker; // 割り込みタイマー 00031 Timer timer; // 時間計測用タイマー 00032 Direction dir = NEUTRAL; // 旋回方向 00033 00034 int i; 00035 uint8_t test_val = 100; 00036 volatile int lps_cnt = 0; // 気圧センサ読み取りカウント 00037 volatile bool INT_flag = true; // 割り込み可否フラグ 00038 /* こちらの変数群はメインループでは参照しない */ 00039 Vector raw_acc(3); // 加速度(m/s^2) 生 00040 Vector raw_gyro(3); // 角速度(deg/s) 生 00041 Vector raw_geomag(3); // 地磁気(?) 生 00042 float raw_press; // 気圧(hPa) 生 00043 float raw_temp; // 温度(℃) 生 00044 /* メインループ内ではこちらを参照する */ 00045 Vector acc(3); // 加速度(m/s^2) 00046 Vector gyro(3); // 角速度(deg/s) 00047 Vector geomag(3); // 地磁気(?) 00048 float p0; // 気圧の初期値 00049 float press; // 気圧(hPa) 00050 float temp; // 温度(℃) 00051 float height; // 高さ(m) 00052 00053 Vector raw_g(3); // 重力ベクトル 生 00054 Vector g(3); // 重力ベクトル 00055 Vector delta_g(3); // Δg 00056 int UTC_t = 0; // UTC時刻 00057 int pre_UTC_t = 0; // 前のUTC時刻 00058 int ss = 0; // 時刻の秒数の小数部分 00059 int boolcheck; //check INT_frag ,true or false 00060 00061 00062 Vector b_f(3); // 機体座標に固定された、機体前方向きのベクトル（x軸） 00063 Vector b_u(3); // 機体座標に固定された、機体上方向きのベクトル（z軸） 00064 Vector b_l(3); // 機体座標に固定された、機体左方向きのベクトル（y軸） 00065 00066 Vector r_f(3); // 世界座標に固定された、北向きのベクトル（X軸） 00067 Vector r_u(3); // 世界座標に固定された、上向きのベクトル（Z軸） 00068 Vector r_l(3); // 世界座標に固定された、西向きのベクトル（Y軸） 00069 00070 float yaw = 0.0f; // 本体のヨー角（deg）z軸周り 00071 float pitch = 0.0f; // 本体のピッチ角（deg）y軸周り 00072 float roll = 0.0f; // 本体のロール角（deg）x軸周り 00073 00074 float vrt_acc = 0.0f; // 鉛直方向の加速度成分（落下検知に使用） 00075 00076 char data[512] = {}; // 送信データ用配 00077 int loopTime = 0; // 1ループに掛かる時間（デバッグ用 00078 float sv = 0.0f; // サーボ電源電圧 00079 float lv = 0.0f; // ロジック電源電圧 00080 00081 #ifdef RULE3_1 00082 float beta = 0.0f; // β変数 （制御則3_1で使用 00083 #endif 00084 /** config.txt ** 00085 * #から始めるのはコメント行 00086 * #イコールの前後に空白を入れない 00087 * target_x=111.222 00088 * target_y=33.444 00089 */ 00090 float target_x, target_y; 00091 00092 /* ---------- Kalman Filter ---------- */ 00093 // 地磁気ベクトル用 00094 // ジャイロのz軸周りのバイアスも推定 00095 Vector pri_x1(7); 00096 Matrix pri_P1(7, 7); 00097 Vector post_x1(7); 00098 Matrix post_P1(7, 7); 00099 Matrix F1(7, 7), H1(3, 7); 00100 Matrix R1(7, 7), Q1(3, 3); 00101 Matrix I1(7, 7); 00102 Matrix K1(7, 3); 00103 Matrix S1(3, 3), S_inv1(3, 3); 00104 00105 // 重力ベクトル用 00106 // ジャイロのx軸、y軸周りのバイアスも同時に推定 00107 Vector pri_x2(5); 00108 Matrix pri_P2(5, 5); 00109 Vector post_x2(5); 00110 Matrix post_P2(5, 5); 00111 Matrix F2(5, 5), H2(3, 5); 00112 Matrix R2(5, 5), Q2(3, 3); 00113 Matrix I2(5, 5); 00114 Matrix K2(5, 3); 00115 Matrix S2(3, 3), S_inv2(3, 3); 00116 /* ---------- ------------- ---------- */ 00117 00118 00119 /****************************** private functions ******************************/ 00120 void SensorsInit(); // 各種センサーの初期化 00121 void KalmanInit(); // カルマンフィルタ初期化 00122 void DataProcessing(); // データ処理関数 00123 void KalmanUpdate(); // カルマンフィルタ更新 00124 void DataUpdate(); // データ取得&更新関数 00125 00126 /** 小さい値を返す関数 00127 * @param a: 1つ目の入力値 00128 * @param b: 2つ目の入力値 00129 * 00130 * @return a,bのうち、小さい方の値 00131 */ 00132 inline float min(float a, float b) 00133 { 00134 return ((a > b) ? b : a); 00135 } 00136 00137 void toString(Vector& v) 00138 { 00139 00140 for(int i=0; i<v.GetDim(); i++) { 00141 // pc.printf("%.6f\t", v.GetComp(i+1)); 00142 } 00143 // pc.printf("\r\n"); 00144 00145 } 00146 00147 /****************************** main function ******************************/ 00148 00149 int main() 00150 { 00151 //重力ベクトルの初期化 00152 raw_g.SetComp(1, 0.0f); 00153 raw_g.SetComp(2, 0.0f); 00154 raw_g.SetComp(3, 1.0f); 00155 //↓mpuのスリープモードを解除するコードを含んだ関数（SensorsInit()）が宣言だけされて実装されてなかったので追加(w) 00156 SensorsInit(); 00157 //↓i2c_mpu.startたしたけど変化なし 00158 i2c_mpu.start(); 00159 i2c_hmc.start(); 00160 i2c_mpu.frequency(400000); 00161 i2c_hmc.frequency(400000); // i2c_hmcの通信速度を400kHzに設定 00162 //カルマンフィルタ初期化 00163 KalmanInit(); 00164 00165 ticker.attach(&DataUpdate, dt); // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み） 00166 00167 /* ------------------------------ ↓↓↓ ここからメインループ ↓↓↓ ------------------------------ */ 00168 while(1) { 00169 timer.stop(); 00170 timer.reset(); 00171 timer.start(); 00172 myled = 1; // LED is ON 00173 INT_flag = true; // 割り込みによる変数書き換えを阻止 00174 /******************************* データ処理 *******************************/ 00175 // DataProcessing(); 00176 /*//check INT_flag true or false(w)================= 00177 if(INT_flag) 00178 boolcheck = 1; 00179 else if(!INT_flag) 00180 boolcheck = 0; 00181 pc.printf("%d",boolcheck); 00182 //==================================================== */ 00183 wait_ms(150); 00184 } 00185 /* ------------------------------ ↑↑↑ ここまでメインループ ↑↑↑ ------------------------------ */ 00186 } 00187 00188 /** 各種センサーの初期化を行う関数 00189 * 00190 */ 00191 void SensorsInit() 00192 { 00193 00194 if(!mpu.init()) error("mpu6050 Initialize Error !!"); // mpu6050初期化 00195 if(!hmc.init()) error("hmc5883l Initialize Error !!"); // hmc5883l初期化 00196 00197 // 機体に固定されたベクトルの初期化 00198 b_f.SetComp(1, 0.0f); 00199 b_f.SetComp(2, 0.0f); 00200 b_f.SetComp(3, -1.0f); 00201 b_u.SetComp(1, 1.0f); 00202 b_u.SetComp(2, 0.0f); 00203 b_u.SetComp(3, 0.0f); 00204 b_l = Cross(b_u, b_f); 00205 00206 // 目標座標をベクトルに代入 00207 // target_p.SetComp(1, target_x * DEG_TO_RAD); 00208 //target_p.SetComp(2, target_y * DEG_TO_RAD); 00209 } 00210 00211 void KalmanInit() 00212 { 00213 // 重力 00214 { 00215 // 誤差共分散行列の値を決める（対角成分のみ） 00216 float alpha_R2 = 0.002f; 00217 float alpha_Q2 = 0.5f; 00218 R2 *= alpha_R2; 00219 Q2 *= alpha_Q2; 00220 00221 // 観測方程式のヤコビアンの値を設定（時間変化無し） 00222 float h2[15] = { 00223 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 00224 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 00225 0.0f, 0.0f, 1.0f, 0.0f, 0.0f 00226 }; 00227 00228 H2.SetComps(h2); 00229 } 00230 00231 // 地磁気 00232 { 00233 // 誤差共分散行列の値を決める（対角成分のみ） 00234 float alpha_R1 = 0.002f; 00235 float alpha_Q1 = 0.5f; 00236 R1 *= alpha_R1; 00237 Q1 *= alpha_Q1; 00238 00239 // 観測方程式のヤコビアンの値を設定（時間変化無し） 00240 float h1[21] = { 00241 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 00242 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 00243 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f 00244 }; 00245 00246 H1.SetComps(h1); 00247 } 00248 } 00249 00250 /** カルマンフィルタの更新を行う関数 00251 * 00252 */ 00253 void KalmanUpdate() 00254 { 00255 // 重力 00256 00257 { 00258 // ヤコビアンの更新 00259 float f2[25] = { 00260 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, 00261 -(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, 00262 (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, 00263 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 00264 0.0f, 0.0f, 0.0f, 0.0f, 1.0f 00265 }; 00266 00267 F2.SetComps(f2); 00268 00269 // 事前推定値の更新 00270 pri_x2 = F2 * post_x2; 00271 00272 float pri_x2_vals[5] = { 00273 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, 00274 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, 00275 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, 00276 post_x2.GetComp(4), 00277 post_x2.GetComp(5) 00278 }; 00279 00280 pri_x2.SetComps(pri_x2_vals); 00281 00282 // 事前誤差分散行列の更新 00283 pri_P2 = F2 * post_P2 * F2.Transpose() + R2; 00284 00285 // カルマンゲインの計算 00286 S2 = Q2 + H2 * pri_P2 * H2.Transpose(); 00287 static float det; 00288 if((det = S2.Inverse(S_inv2)) >= 0.0f) { 00289 pc.printf("E:%.3f\r\n", det); 00290 return; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了 00291 } 00292 K2 = pri_P2 * H2.Transpose() * S_inv2; 00293 00294 // 事後推定値の更新 00295 post_x2 = pri_x2 + K2 * (raw_acc - H2 * pri_x2); 00296 // 事後誤差分散行列の更新 00297 post_P2 = (I2 - K2 * H2) * pri_P2; 00298 } 00299 00300 00301 // 地磁気 00302 { 00303 // ヤコビアンの更新 00304 float f1[49] = { 00305 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, 00306 -(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, 00307 (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, 00308 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 00309 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 00310 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 00311 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f 00312 }; 00313 00314 F1.SetComps(f1); 00315 00316 // 事前推定値の更新 00317 //pri_x1 = F1 * post_x1; 00318 float pri_x1_vals[7] = { 00319 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, 00320 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, 00321 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, 00322 post_x1.GetComp(4), 00323 post_x1.GetComp(5), 00324 post_x1.GetComp(6), 00325 post_x1.GetComp(7) 00326 }; 00327 00328 pri_x1.SetComps(pri_x1_vals); 00329 00330 // 事前誤差分散行列の更新 00331 pri_P1 = F1 * post_P1 * F1.Transpose() + R1; 00332 00333 // カルマンゲインの計算 00334 S1 = Q1 + H1 * pri_P1 * H1.Transpose(); 00335 static float det; 00336 if((det = S1.Inverse(S_inv1)) >= 0.0f) { 00337 pc.printf("E:%.3f\r\n", det); 00338 return; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了 00339 } 00340 K1 = pri_P1 * H1.Transpose() * S_inv1; 00341 00342 // 事後推定値の更新 00343 post_x1 = pri_x1 + K1 * (raw_geomag - H1 * pri_x1); 00344 // 事後誤差分散行列の更新 00345 post_P1 = (I1 - K1 * H1) * pri_P1; 00346 } 00347 } 00348 00349 /* ------------------------------ 割り込み関数 ------------------------------ */ 00350 00351 /** データ取得＆更新関数 00352 * 00353 */ 00354 void DataUpdate() 00355 { 00356 // センサーの値を更新 00357 mpu.read(); 00358 hmc.read(); 00359 /*//↓i2c_mpuの通信に成功しているかどうかを確認（w） 00360 //=========================================================== 00361 if(mpu.checker_get() == 0){ 00362 pc.printf("SUCCESS!!\n\r"); 00363 } 00364 for(i = 0;i < 13;i++){ 00365 pc.printf("%u\n\r",mpu.data.reg[i]); 00366 } 00367 //============================================================ 00368 */ 00369 for(int i=0; i<3; i++) { 00370 raw_acc.SetComp(i+1, (float)mpu.data.value.acc[i] * ACC_LSB_TO_G); 00371 raw_gyro.SetComp(i+1, (float)mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD); 00372 raw_geomag.SetComp(i+1, (float)hmc.data.value[i] * MAG_LSB_TO_GAUSS); 00373 //pc.printf("raw_acc/%d:%10f raw_gyro/%d:%10f\n\r",i,raw_acc.GetComp(i), i,raw_gyro.GetComp(i)); 00374 } 00375 raw_g.SetComp(1, 0.0f); 00376 raw_g.SetComp(2, 0.0f); 00377 raw_g.SetComp(3, 1.0f); 00378 delta_g = Cross(raw_gyro, raw_g); // Δg = ω × g 00379 //for(int i = 0;i < 3;i++){ 00380 // pc.printf("delta_g%d : %7f\n\r",i,delta_g.GetComp(i)); 00381 // } 00382 //for(int i = 0; i < delta_g.Getdim(); i++) { 00383 // pc.printf("delta_g%d:%f\n\r",i,delta_g.GetComp(i)); 00384 // } 00385 raw_g = 0.9f * (raw_g - delta_g * dt) + 0.1f * raw_acc.Normalize(); // 相補フィルタ 00386 /* for(int i = 1; i <= raw_g.Getdim(); i++) { 00387 pc.printf("%f",raw_g.GetComp(i)); 00388 } 00389 */ 00390 raw_g = raw_g.Normalize(); 00391 //========================================= 00392 00393 KalmanUpdate(); 00394 /*//check INT_flag true or false(w)================= 00395 if(INT_flag) 00396 boolcheck = 1; 00397 else if(!INT_flag) 00398 boolcheck = 0; 00399 pc.printf("%d",boolcheck); 00400 //====================================================*/ 00401 if(1) { 00402 00403 g = raw_g; 00404 for(int i=0; i<3; i++) { 00405 geomag.SetComp(i+1, post_x1.GetComp(i+1)); 00406 } 00407 acc = raw_acc; 00408 gyro = raw_gyro; 00409 press = raw_press; 00410 temp = raw_temp; 00411 00412 vrt_acc = raw_acc * raw_g; 00413 } 00414 static float R_11; // 回転行列(1,1)成分 00415 static float R_12; // 回転行列(1,2)成分 00416 static float r_cos; // ロール角のcos値 00417 static float r_sin; // ロール角のsin値 00418 static float p_cos; // ピッチ角のcos値 00419 static float p_sin; // ピッチ角のsin値 00420 00421 00422 // 参照座標系の基底を求める 00423 r_u = g; 00424 r_l = Cross(r_u, r_f); 00425 r_f = geomag.GetPerpCompTo(g).Normalize(); 00426 00427 // 回転行列を経由してオイラー角を求める 00428 // オイラー角はヨー・ピッチ・ロールの順に回転したものとする 00429 // 各オイラー角を求めるのに回転行列の全成分は要らないので、一部だけ計算する。 00430 00431 R_11 = r_f * b_f; // 回転行列の(1,1)成分 00432 R_12 = r_f * b_l; // 回転行列の(1,2)成分 00433 yaw = atan2(-R_12, R_11) * RAD_TO_DEG + MAG_DECLINATION; 00434 r_cos = g.GetPerpCompTo(b_f).Normalize() * b_u; 00435 r_sin = Cross(g.GetPerpCompTo(b_f).Normalize(), b_u) * b_f; 00436 if(r_sin > 0.0f) { 00437 roll = acos(r_cos) * RAD_TO_DEG; 00438 } else { 00439 roll = -acos(r_cos) * RAD_TO_DEG; 00440 } 00441 p_cos = g.GetPerpCompTo(b_l).Normalize() * b_u; 00442 p_sin = Cross(g.GetPerpCompTo(b_l).Normalize(), b_u) * b_l; 00443 00444 if(p_sin > 0.0f) { 00445 pitch = acos(p_cos) * RAD_TO_DEG; 00446 } else { 00447 pitch = -acos(p_cos) * RAD_TO_DEG; 00448 } 00449 00450 if(yaw > 180.0f) yaw -= 360.0f; // ヨー角を[-π, π]に補正 00451 else if(yaw < -180.0f) yaw += 360.0f; // ヨー角を[-π, π]に補正 00452 //下のroll,pitch,yawを表示させるprintfでnanと出るので、変数の中に何も値が代入されていない可能性を調べた。→値は入ってるらしい。(w)======================== 00453 pc.printf("pitch:%7f roll:%7f yaw:%7f\n\r",pitch,roll,yaw); 00454 }
Generated on Tue Jul 12 2022 22:15:02 by
1.7.2