hiroya taura
/
LAURUS_program
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 
LAURUS
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(p9, p8); // I2Cポート 00023 MPU6050 mpu(&i2c); // 加速度・角速度センサ 00024 HMC5883L hmc(&i2c); // 地磁気センサ 00025 LPS25H lps(&i2c); // 気圧センサ 00026 Serial pc(USBTX,USBRX); // PC通信用シリアルポート 00027 FILE * fp; // ログファイルのポインタ 00028 ConfigFile cfg; // ConfigFile 00029 Ticker ticker; // 割り込みタイマー 00030 Timer timer; // 時間計測用タイマー 00031 Direction dir = NEUTRAL; // 旋回方向 00032 00033 volatile int lps_cnt = 0; // 気圧センサ読み取りカウント 00034 volatile bool INT_flag = true; // 割り込み可否フラグ 00035 /* こちらの変数群はメインループでは参照しない */ 00036 Vector raw_acc(3); // 加速度(m/s^2) 生 00037 Vector raw_gyro(3); // 角速度(deg/s) 生 00038 Vector raw_geomag(3); // 地磁気(?) 生 00039 float raw_press; // 気圧(hPa) 生 00040 float raw_temp; // 温度(℃) 生 00041 /* メインループ内ではこちらを参照する */ 00042 Vector acc(3); // 加速度(m/s^2) 00043 Vector gyro(3); // 角速度(deg/s) 00044 Vector geomag(3); // 地磁気(?) 00045 float p0; // 気圧の初期値 00046 float press; // 気圧(hPa) 00047 float temp; // 温度(℃) 00048 float height; // 高さ(m) 00049 00050 Vector raw_g(3); // 重力ベクトル 生 00051 Vector g(3); // 重力ベクトル 00052 Vector target_p(2); // 目標情報（経度、緯度）（rad） 00053 Vector p(2); // 現在の位置情報(補完含む)(経度, 緯度)（rad） 00054 Vector new_p(2); // 最新の位置情報(経度, 緯度)（rad） 00055 Vector pre_p(2); // 過去の位置情報(経度, 緯度)（rad） 00056 int UTC_t = 0; // UTC時刻 00057 int pre_UTC_t = 0; // 前のUTC時刻 00058 int ss = 0; // 時刻の秒数の小数部分 00059 00060 Vector b_f(3); // 機体座標に固定された、機体前方向きのベクトル（x軸） 00061 Vector b_u(3); // 機体座標に固定された、機体上方向きのベクトル（z軸） 00062 Vector b_l(3); // 機体座標に固定された、機体左方向きのベクトル（y軸） 00063 00064 Vector r_f(3); // 世界座標に固定された、北向きのベクトル（X軸） 00065 Vector r_u(3); // 世界座標に固定された、上向きのベクトル（Z軸） 00066 Vector r_l(3); // 世界座標に固定された、西向きのベクトル（Y軸） 00067 00068 float pull_L = 10.0f; // 左サーボの引っ張り量（目標値）(mm：0~PullMax) 00069 float pull_R = 10.0f; // 右サーボの引っ張り量（目標値）(mm：0~PullMax) 00070 00071 float yaw = 0.0f; // 本体のヨー角（deg）z軸周り 00072 float pitch = 0.0f; // 本体のピッチ角（deg）y軸周り 00073 float roll = 0.0f; // 本体のロール角（deg）x軸周り 00074 00075 float vrt_acc = 0.0f; // 鉛直方向の加速度成分（落下検知に使用） 00076 00077 volatile int step = 0; // シーケンス制御のステップ 00078 char data[512] = {}; // 送信データ用配 00079 int loopTime = 0; // 1ループに掛かる時間（デバッグ用 00080 float sv = 0.0f; // サーボ電源電圧 00081 float lv = 0.0f; // ロジック電源電圧 00082 00083 #ifdef RULE3_1 00084 float beta = 0.0f; // β変数 （制御則3_1で使用 00085 #endif 00086 /** config.txt ** 00087 * #から始めるのはコメント行 00088 * #イコールの前後に空白を入れない 00089 * target_x=111.222 00090 * target_y=33.444 00091 */ 00092 float target_x, target_y; 00093 00094 /* ---------- Kalman Filter ---------- */ 00095 // 地磁気ベクトル用 00096 // ジャイロのz軸周りのバイアスも推定 00097 Vector pri_x1(7); 00098 Matrix pri_P1(7, 7); 00099 Vector post_x1(7); 00100 Matrix post_P1(7, 7); 00101 Matrix F1(7, 7), H1(3, 7); 00102 Matrix R1(7, 7), Q1(3, 3); 00103 Matrix I1(7, 7); 00104 Matrix K1(7, 3); 00105 Matrix S1(3, 3), S_inv1(3, 3); 00106 00107 // 重力ベクトル用 00108 // ジャイロのx軸、y軸周りのバイアスも同時に推定 00109 Vector pri_x2(5); 00110 Matrix pri_P2(5, 5); 00111 Vector post_x2(5); 00112 Matrix post_P2(5, 5); 00113 Matrix F2(5, 5), H2(3, 5); 00114 Matrix R2(5, 5), Q2(3, 3); 00115 Matrix I2(5, 5); 00116 Matrix K2(5, 3); 00117 Matrix S2(3, 3), S_inv2(3, 3); 00118 /* ---------- ------------- ---------- */ 00119 00120 00121 /****************************** private functions ******************************/ 00122 void SensorsInit(); // 各種センサーの初期化 00123 void KalmanInit(); // カルマンフィルタ初期化 00124 void DataProcessing(); // データ処理関数 00125 void KalmanUpdate(); // カルマンフィルタ更新 00126 void DataUpdate(); // データ取得&更新関数 00127 00128 /** 小さい値を返す関数 00129 * @param a: 1つ目の入力値 00130 * @param b: 2つ目の入力値 00131 * 00132 * @return a,bのうち、小さい方の値 00133 */ 00134 inline float min(float a, float b) 00135 { 00136 return ((a > b) ? b : a); 00137 } 00138 00139 /****************************** main function ******************************/ 00140 00141 int main() 00142 { 00143 00144 i2c.frequency(400000); // I2Cの通信速度を400kHzに設定 00145 //カルマンフィルタ初期化 00146 KalmanInit(); 00147 00148 ticker.attach(&DataUpdate, dt); // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み） 00149 00150 00151 00152 /* ------------------------------ ↓↓↓ ここからメインループ ↓↓↓ ------------------------------ */ 00153 while(1) { 00154 timer.stop(); 00155 timer.reset(); 00156 timer.start(); 00157 myled = 1; // LED is ON 00158 00159 INT_flag = false; // 割り込みによる変数書き換えを阻止 00160 /******************************* データ処理 *******************************/ 00161 DataProcessing(); 00162 INT_flag = true; 00163 } 00164 /* ------------------------------ ↑↑↑ ここまでメインループ ↑↑↑ ------------------------------ */ 00165 } 00166 00167 /** データ処理関数 00168 * 00169 */ 00170 void DataProcessing() 00171 { 00172 static float R_11; // 回転行列(1,1)成分 00173 static float R_12; // 回転行列(1,2)成分 00174 static float r_cos; // ロール角のcos値 00175 static float r_sin; // ロール角のsin値 00176 static float p_cos; // ピッチ角のcos値 00177 static float p_sin; // ピッチ角のsin値 00178 00179 00180 // 参照座標系の基底を求める 00181 r_u = g; 00182 r_f = geomag.GetPerpCompTo(g).Normalize(); 00183 r_l = Cross(r_u, r_f); 00184 00185 // 回転行列を経由してオイラー角を求める 00186 // オイラー角はヨー・ピッチ・ロールの順に回転したものとする 00187 // 各オイラー角を求めるのに回転行列の全成分は要らないので、一部だけ計算する。 00188 00189 R_11 = r_f * b_f; // 回転行列の(1,1)成分 00190 R_12 = r_f * b_l; // 回転行列の(1,2)成分 00191 yaw = atan2(-R_12, R_11) * RAD_TO_DEG + MAG_DECLINATION; 00192 r_cos = g.GetPerpCompTo(b_f).Normalize() * b_u; 00193 r_sin = Cross(g.GetPerpCompTo(b_f).Normalize(), b_u) * b_f; 00194 if(r_sin > 0.0f) { 00195 roll = acos(r_cos) * RAD_TO_DEG; 00196 } else { 00197 roll = -acos(r_cos) * RAD_TO_DEG; 00198 } 00199 00200 p_cos = g.GetPerpCompTo(b_l).Normalize() * b_u; 00201 p_sin = Cross(g.GetPerpCompTo(b_l).Normalize(), b_u) * b_l; 00202 if(p_sin > 0.0f) { 00203 pitch = acos(p_cos) * RAD_TO_DEG; 00204 } else { 00205 pitch = -acos(p_cos) * RAD_TO_DEG; 00206 } 00207 00208 if(yaw > 180.0f) yaw -= 360.0f; // ヨー角を[-π, π]に補正 00209 else if(yaw < -180.0f) yaw += 360.0f; // ヨー角を[-π, π]に補正 00210 } 00211 00212 00213 /** 各種センサーの初期化を行う関数 00214 * 00215 */ 00216 void SensorsInit() 00217 { 00218 00219 if(!mpu.init()) error("mpu6050 Initialize Error !!"); // mpu6050初期化 00220 if(!hmc.init()) error("hmc5883l Initialize Error !!"); // hmc5883l初期化 00221 00222 //重力ベクトルの初期化 00223 raw_g.SetComp(1, 0.0f); 00224 raw_g.SetComp(2, 0.0f); 00225 raw_g.SetComp(3, 1.0f); 00226 00227 // 機体に固定されたベクトルの初期化 00228 b_f.SetComp(1, 0.0f); 00229 b_f.SetComp(2, 0.0f); 00230 b_f.SetComp(3, -1.0f); 00231 b_u.SetComp(1, 1.0f); 00232 b_u.SetComp(2, 0.0f); 00233 b_u.SetComp(3, 0.0f); 00234 b_l = Cross(b_u, b_f); 00235 00236 // 目標座標をベクトルに代入 00237 target_p.SetComp(1, target_x * DEG_TO_RAD); 00238 target_p.SetComp(2, target_y * DEG_TO_RAD); 00239 } 00240 00241 void KalmanInit() 00242 { 00243 // 重力 00244 { 00245 // 誤差共分散行列の値を決める（対角成分のみ） 00246 float alpha_R2 = 0.002f; 00247 float alpha_Q2 = 0.5f; 00248 R2 *= alpha_R2; 00249 Q2 *= alpha_Q2; 00250 00251 // 観測方程式のヤコビアンの値を設定（時間変化無し） 00252 float h2[15] = { 00253 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 00254 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 00255 0.0f, 0.0f, 1.0f, 0.0f, 0.0f 00256 }; 00257 00258 H2.SetComps(h2); 00259 } 00260 00261 // 地磁気 00262 { 00263 // 誤差共分散行列の値を決める（対角成分のみ） 00264 float alpha_R1 = 0.002f; 00265 float alpha_Q1 = 0.5f; 00266 R1 *= alpha_R1; 00267 Q1 *= alpha_Q1; 00268 00269 // 観測方程式のヤコビアンの値を設定（時間変化無し） 00270 float h1[21] = { 00271 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 00272 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 00273 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f 00274 }; 00275 00276 H1.SetComps(h1); 00277 } 00278 } 00279 00280 /** カルマンフィルタの更新を行う関数 00281 * 00282 */ 00283 void KalmanUpdate() 00284 { 00285 // 重力 00286 00287 { 00288 // ヤコビアンの更新 00289 float f2[25] = { 00290 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, 00291 -(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, 00292 (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, 00293 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 00294 0.0f, 0.0f, 0.0f, 0.0f, 1.0f 00295 }; 00296 00297 F2.SetComps(f2); 00298 00299 // 事前推定値の更新 00300 //pri_x2 = F2 * post_x2; 00301 00302 float pri_x2_vals[5] = { 00303 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, 00304 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, 00305 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, 00306 post_x2.GetComp(4), 00307 post_x2.GetComp(5) 00308 }; 00309 00310 pri_x2.SetComps(pri_x2_vals); 00311 00312 // 事前誤差分散行列の更新 00313 pri_P2 = F2 * post_P2 * F2.Transpose() + R2; 00314 00315 // カルマンゲインの計算 00316 S2 = Q2 + H2 * pri_P2 * H2.Transpose(); 00317 static float det; 00318 if((det = S2.Inverse(S_inv2)) >= 0.0f) { 00319 pc.printf("E:%.3f\r\n", det); 00320 return; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了 00321 } 00322 K2 = pri_P2 * H2.Transpose() * S_inv2; 00323 00324 // 事後推定値の更新 00325 post_x2 = pri_x2 + K2 * (raw_acc - H2 * pri_x2); 00326 // 事後誤差分散行列の更新 00327 post_P2 = (I2 - K2 * H2) * pri_P2; 00328 } 00329 00330 00331 // 地磁気 00332 { 00333 // ヤコビアンの更新 00334 float f1[49] = { 00335 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, 00336 -(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, 00337 (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, 00338 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 00339 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 00340 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 00341 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f 00342 }; 00343 00344 F1.SetComps(f1); 00345 00346 // 事前推定値の更新 00347 //pri_x1 = F1 * post_x1; 00348 float pri_x1_vals[7] = { 00349 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, 00350 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, 00351 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, 00352 post_x1.GetComp(4), 00353 post_x1.GetComp(5), 00354 post_x1.GetComp(6), 00355 post_x1.GetComp(7) 00356 }; 00357 00358 pri_x1.SetComps(pri_x1_vals); 00359 00360 // 事前誤差分散行列の更新 00361 pri_P1 = F1 * post_P1 * F1.Transpose() + R1; 00362 00363 // カルマンゲインの計算 00364 S1 = Q1 + H1 * pri_P1 * H1.Transpose(); 00365 static float det; 00366 if((det = S1.Inverse(S_inv1)) >= 0.0f) { 00367 pc.printf("E:%.3f\r\n", det); 00368 return; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了 00369 } 00370 K1 = pri_P1 * H1.Transpose() * S_inv1; 00371 00372 // 事後推定値の更新 00373 post_x1 = pri_x1 + K1 * (raw_geomag - H1 * pri_x1); 00374 // 事後誤差分散行列の更新 00375 post_P1 = (I1 - K1 * H1) * pri_P1; 00376 } 00377 } 00378 00379 /* ------------------------------ 割り込み関数 ------------------------------ */ 00380 00381 /** データ取得＆更新関数 00382 * 00383 */ 00384 void DataUpdate() 00385 { 00386 // センサーの値を更新 00387 mpu.read(); 00388 hmc.read(); 00389 00390 for(int i=0; i<3; i++) { 00391 raw_acc.SetComp(i+1, (float)mpu.data.value.acc[i] * ACC_LSB_TO_G); 00392 raw_gyro.SetComp(i+1, (float)mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD); 00393 raw_geomag.SetComp(i+1, (float)hmc.data.value[i] * MAG_LSB_TO_GAUSS); 00394 } 00395 00396 Vector delta_g = Cross(raw_gyro, raw_g); // Δg = ω × g 00397 raw_g = 0.9f * (raw_g - delta_g * dt) + 0.1f * raw_acc.Normalize(); // 相補フィルタ 00398 raw_g = raw_g.Normalize(); 00399 00400 KalmanUpdate(); 00401 00402 if(INT_flag) { 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 }
Generated on Thu Jul 14 2022 07:53:07 by
1.7.2