albatross / Mbed 2 deprecated LAURUS_program_copy

MPUとHMCでうごくかもver

Dependencies:   ConfigFile SDFileSystem mbed

Fork of LAURUS_program by hiroya taura

main.cpp@39:9ca388ce902f, 2015-12-23 (annotated)

Committer:
YusukeWakuta
Date:
Wed Dec 23 09:59:35 2015 +0000
Revision:
39:9ca388ce902f
Parent:
38:ada39f1c6c76
Child:
40:4188d55e62fc
vector????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ojan 0:bc6f14fc60c7 1 #include "mbed.h"
ojan 0:bc6f14fc60c7 2 #include "MPU6050.h"
YusukeWakuta 38:ada39f1c6c76 3 //#include "HMC5883L.h"
ojan 0:bc6f14fc60c7 4 #include "LPS25H.h"
ojan 1:6cd6d2760856 5 #include "GMS6_CR6.h"
ojan 0:bc6f14fc60c7 6 #include "Vector.h"
ojan 3:5358a691a100 7 #include "Matrix.h"
ojan 3:5358a691a100 8 #include "Vector_Matrix_operator.h"
ojan 0:bc6f14fc60c7 9 #include "myConstants.h"
onaka 7:0ec343d29641 10 #include "SDFileSystem.h"
onaka 7:0ec343d29641 11 #include "BufferedSerial.h"
onaka 7:0ec343d29641 12 #include "ConfigFile.h"
ojan 0:bc6f14fc60c7 13
ojan 14:f85cb5340cb8 14 const float dt = 0.01f; // 割り込み周期(s)
ojan 1:6cd6d2760856 15
ojan 25:4c72d7420d8a 16 enum Direction {LEFT, RIGHT, NEUTRAL};
ojan 25:4c72d7420d8a 17
ojan 14:f85cb5340cb8 18 /****************************** private macro ******************************/
ojan 14:f85cb5340cb8 19 /****************************** private typedef ******************************/
ojan 14:f85cb5340cb8 20 /****************************** private variables ******************************/
ojan 10:8ee11e412ad7 21 DigitalOut myled(LED1); // デバッグ用LEDのためのデジタル出力
YusukeWakuta 37:97152b80d28c 22 I2C i2c(p9,p10); // I2Cポート
ojan 10:8ee11e412ad7 23 MPU6050 mpu(&i2c); // 加速度・角速度センサ
YusukeWakuta 38:ada39f1c6c76 24 //HMC5883L hmc(&i2c); // 地磁気センサ
ojan 10:8ee11e412ad7 25 LPS25H lps(&i2c); // 気圧センサ
taurin 36:94dc027e05cd 26 Serial pc(USBTX,USBRX); // PC通信用シリアルポート
ojan 19:ad8ff2de68f5 27 FILE * fp; // ログファイルのポインタ
ojan 14:f85cb5340cb8 28 ConfigFile cfg; // ConfigFile
ojan 20:00afba164688 29 Ticker ticker; // 割り込みタイマー
ojan 14:f85cb5340cb8 30 Timer timer; // 時間計測用タイマー
ojan 25:4c72d7420d8a 31 Direction dir = NEUTRAL; // 旋回方向
ojan 0:bc6f14fc60c7 32
YusukeWakuta 38:ada39f1c6c76 33 int i;
YusukeWakuta 38:ada39f1c6c76 34 uint8_t test_val = 100;
ojan 34:4bda9af9a0cd 35 volatile int lps_cnt = 0; // 気圧センサ読み取りカウント
ojan 34:4bda9af9a0cd 36 volatile bool INT_flag = true; // 割り込み可否フラグ
ojan 17:03b45055ca05 37 /* こちらの変数群はメインループでは参照しない */
ojan 14:f85cb5340cb8 38 Vector raw_acc(3); // 加速度(m/s^2) 生
ojan 14:f85cb5340cb8 39 Vector raw_gyro(3); // 角速度(deg/s) 生
ojan 14:f85cb5340cb8 40 Vector raw_geomag(3); // 地磁気(?) 生
ojan 14:f85cb5340cb8 41 float raw_press; // 気圧(hPa) 生
ojan 23:79cdc1432160 42 float raw_temp; // 温度(℃)　生
ojan 17:03b45055ca05 43 /* メインループ内ではこちらを参照する */
ojan 14:f85cb5340cb8 44 Vector acc(3); // 加速度(m/s^2)
ojan 14:f85cb5340cb8 45 Vector gyro(3); // 角速度(deg/s)
ojan 14:f85cb5340cb8 46 Vector geomag(3); // 地磁気(?)
ojan 24:8838be99cec3 47 float p0; // 気圧の初期値
ojan 14:f85cb5340cb8 48 float press; // 気圧(hPa)
ojan 23:79cdc1432160 49 float temp; // 温度(℃)
ojan 24:8838be99cec3 50 float height; // 高さ(m)
ojan 1:6cd6d2760856 51
ojan 14:f85cb5340cb8 52 Vector raw_g(3); // 重力ベクトル 生
ojan 14:f85cb5340cb8 53 Vector g(3); // 重力ベクトル
ojan 14:f85cb5340cb8 54 int UTC_t = 0; // UTC時刻
ojan 14:f85cb5340cb8 55 int pre_UTC_t = 0; // 前のUTC時刻
ojan 30:fb310564097b 56 int ss = 0; // 時刻の秒数の小数部分
YusukeWakuta 39:9ca388ce902f 57 int boolcheck; //check INT_frag ,true or false
YusukeWakuta 39:9ca388ce902f 58
ojan 8:602865d8fca3 59
ojan 14:f85cb5340cb8 60 Vector b_f(3); // 機体座標に固定された、機体前方向きのベクトル（x軸）
ojan 14:f85cb5340cb8 61 Vector b_u(3); // 機体座標に固定された、機体上方向きのベクトル（z軸）
ojan 14:f85cb5340cb8 62 Vector b_l(3); // 機体座標に固定された、機体左方向きのベクトル（y軸）
ojan 9:6d4578dcc1ed 63
ojan 14:f85cb5340cb8 64 Vector r_f(3); // 世界座標に固定された、北向きのベクトル（X軸）
ojan 14:f85cb5340cb8 65 Vector r_u(3); // 世界座標に固定された、上向きのベクトル（Z軸）
ojan 14:f85cb5340cb8 66 Vector r_l(3); // 世界座標に固定された、西向きのベクトル（Y軸）
ojan 14:f85cb5340cb8 67
ojan 14:f85cb5340cb8 68 float yaw = 0.0f; // 本体のヨー角（deg）z軸周り
ojan 14:f85cb5340cb8 69 float pitch = 0.0f; // 本体のピッチ角（deg）y軸周り
ojan 14:f85cb5340cb8 70 float roll = 0.0f; // 本体のロール角（deg）x軸周り
ojan 9:6d4578dcc1ed 71
ojan 14:f85cb5340cb8 72 float vrt_acc = 0.0f; // 鉛直方向の加速度成分（落下検知に使用）
ojan 13:df1e8a650185 73
ojan 20:00afba164688 74 char data[512] = {}; // 送信データ用配
ojan 20:00afba164688 75 int loopTime = 0; // 1ループに掛かる時間（デバッグ用
ojan 25:4c72d7420d8a 76 float sv = 0.0f; // サーボ電源電圧
ojan 25:4c72d7420d8a 77 float lv = 0.0f; // ロジック電源電圧
ojan 3:5358a691a100 78
ojan 34:4bda9af9a0cd 79 #ifdef RULE3_1
ojan 34:4bda9af9a0cd 80 float beta = 0.0f; // β変数 （制御則3_1で使用
ojan 34:4bda9af9a0cd 81 #endif
onaka 7:0ec343d29641 82 /** config.txt **
ojan 15:d14d385d37e2 83 * #から始めるのはコメント行
onaka 7:0ec343d29641 84 * #イコールの前後に空白を入れない
onaka 7:0ec343d29641 85 * target_x=111.222
onaka 7:0ec343d29641 86 * target_y=33.444
onaka 7:0ec343d29641 87 */
ojan 23:79cdc1432160 88 float target_x, target_y;
onaka 7:0ec343d29641 89
ojan 14:f85cb5340cb8 90 /* ---------- Kalman Filter ---------- */
ojan 11:083c8c9a5b84 91 // 地磁気ベクトル用
ojan 14:f85cb5340cb8 92 // ジャイロのz軸周りのバイアスも推定
ojan 13:df1e8a650185 93 Vector pri_x1(7);
ojan 13:df1e8a650185 94 Matrix pri_P1(7, 7);
ojan 13:df1e8a650185 95 Vector post_x1(7);
ojan 13:df1e8a650185 96 Matrix post_P1(7, 7);
ojan 13:df1e8a650185 97 Matrix F1(7, 7), H1(3, 7);
ojan 13:df1e8a650185 98 Matrix R1(7, 7), Q1(3, 3);
ojan 13:df1e8a650185 99 Matrix I1(7, 7);
ojan 13:df1e8a650185 100 Matrix K1(7, 3);
ojan 11:083c8c9a5b84 101 Matrix S1(3, 3), S_inv1(3, 3);
ojan 11:083c8c9a5b84 102
ojan 11:083c8c9a5b84 103 // 重力ベクトル用
ojan 14:f85cb5340cb8 104 // ジャイロのx軸、y軸周りのバイアスも同時に推定
ojan 13:df1e8a650185 105 Vector pri_x2(5);
ojan 13:df1e8a650185 106 Matrix pri_P2(5, 5);
ojan 13:df1e8a650185 107 Vector post_x2(5);
ojan 13:df1e8a650185 108 Matrix post_P2(5, 5);
ojan 13:df1e8a650185 109 Matrix F2(5, 5), H2(3, 5);
ojan 13:df1e8a650185 110 Matrix R2(5, 5), Q2(3, 3);
ojan 13:df1e8a650185 111 Matrix I2(5, 5);
ojan 13:df1e8a650185 112 Matrix K2(5, 3);
ojan 13:df1e8a650185 113 Matrix S2(3, 3), S_inv2(3, 3);
ojan 14:f85cb5340cb8 114 /* ---------- ------------- ---------- */
ojan 3:5358a691a100 115
ojan 1:6cd6d2760856 116
ojan 14:f85cb5340cb8 117 /****************************** private functions ******************************/
ojan 23:79cdc1432160 118 void SensorsInit(); // 各種センサーの初期化
ojan 17:03b45055ca05 119 void KalmanInit(); // カルマンフィルタ初期化
ojan 20:00afba164688 120 void DataProcessing(); // データ処理関数
ojan 14:f85cb5340cb8 121 void KalmanUpdate(); // カルマンフィルタ更新
ojan 20:00afba164688 122 void DataUpdate(); // データ取得&更新関数
ojan 1:6cd6d2760856 123
ojan 25:4c72d7420d8a 124 /** 小さい値を返す関数
ojan 25:4c72d7420d8a 125 * @param a: 1つ目の入力値
ojan 25:4c72d7420d8a 126 * @param b: 2つ目の入力値
taurin 36:94dc027e05cd 127 *
ojan 25:4c72d7420d8a 128 * @return a,bのうち、小さい方の値
ojan 25:4c72d7420d8a 129 */
ojan 15:d14d385d37e2 130 inline float min(float a, float b)
ojan 15:d14d385d37e2 131 {
ojan 14:f85cb5340cb8 132 return ((a > b) ? b : a);
ojan 14:f85cb5340cb8 133 }
ojan 14:f85cb5340cb8 134
ojan 14:f85cb5340cb8 135 /****************************** main function ******************************/
ojan 0:bc6f14fc60c7 136
ojan 15:d14d385d37e2 137 int main()
YusukeWakuta 37:97152b80d28c 138
ojan 15:d14d385d37e2 139 {
YusukeWakuta 38:ada39f1c6c76 140 //↓mpuのスリープモードを解除するコードを含んだ関数（SensorsInit()）が宣言だけされて実装されてなかったので追加(w)
YusukeWakuta 38:ada39f1c6c76 141 SensorsInit();
YusukeWakuta 38:ada39f1c6c76 142 i2c.start();
YusukeWakuta 37:97152b80d28c 143 pc.printf("HELLO");
ojan 14:f85cb5340cb8 144 i2c.frequency(400000); // I2Cの通信速度を400kHzに設定
onaka 7:0ec343d29641 145 //カルマンフィルタ初期化
ojan 3:5358a691a100 146 KalmanInit();
ojan 15:d14d385d37e2 147
ojan 20:00afba164688 148 ticker.attach(&DataUpdate, dt); // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
taurin 36:94dc027e05cd 149
ojan 14:f85cb5340cb8 150 /* ------------------------------ ↓↓↓ ここからメインループ ↓↓↓ ------------------------------ */
ojan 0:bc6f14fc60c7 151 while(1) {
YusukeWakuta 38:ada39f1c6c76 152 // pc.printf("HELLO");
ojan 4:45dc5590abc0 153 timer.stop();
ojan 4:45dc5590abc0 154 timer.reset();
ojan 4:45dc5590abc0 155 timer.start();
ojan 14:f85cb5340cb8 156 myled = 1; // LED is ON
taurin 36:94dc027e05cd 157
ojan 24:8838be99cec3 158 INT_flag = false; // 割り込みによる変数書き換えを阻止
ojan 14:f85cb5340cb8 159 /******************************* データ処理 *******************************/
ojan 20:00afba164688 160 DataProcessing();
taurin 36:94dc027e05cd 161 INT_flag = true;
YusukeWakuta 39:9ca388ce902f 162 /*//check INT_flag true or false(w)=================
YusukeWakuta 39:9ca388ce902f 163 if(INT_flag)
YusukeWakuta 39:9ca388ce902f 164 boolcheck = 1;
YusukeWakuta 39:9ca388ce902f 165 else if(!INT_flag)
YusukeWakuta 39:9ca388ce902f 166 boolcheck = 0;
YusukeWakuta 39:9ca388ce902f 167 pc.printf("%d",boolcheck);
YusukeWakuta 39:9ca388ce902f 168 //==================================================== */
ojan 0:bc6f14fc60c7 169 }
ojan 14:f85cb5340cb8 170 /* ------------------------------ ↑↑↑ ここまでメインループ ↑↑↑ ------------------------------ */
onaka 7:0ec343d29641 171 }
onaka 7:0ec343d29641 172
ojan 20:00afba164688 173 /** データ処理関数
ojan 20:00afba164688 174 *
ojan 20:00afba164688 175 */
ojan 20:00afba164688 176 void DataProcessing()
ojan 20:00afba164688 177 {
ojan 27:a26ff85bba23 178 static float R_11; // 回転行列(1,1)成分
ojan 27:a26ff85bba23 179 static float R_12; // 回転行列(1,2)成分
ojan 27:a26ff85bba23 180 static float r_cos; // ロール角のcos値
ojan 27:a26ff85bba23 181 static float r_sin; // ロール角のsin値
ojan 27:a26ff85bba23 182 static float p_cos; // ピッチ角のcos値
ojan 27:a26ff85bba23 183 static float p_sin; // ピッチ角のsin値
taurin 36:94dc027e05cd 184
ojan 20:00afba164688 185
ojan 20:00afba164688 186 // 参照座標系の基底を求める
ojan 20:00afba164688 187 r_u = g;
ojan 20:00afba164688 188 r_f = geomag.GetPerpCompTo(g).Normalize();
ojan 20:00afba164688 189 r_l = Cross(r_u, r_f);
ojan 20:00afba164688 190
ojan 20:00afba164688 191 // 回転行列を経由してオイラー角を求める
ojan 20:00afba164688 192 // オイラー角はヨー・ピッチ・ロールの順に回転したものとする
ojan 20:00afba164688 193 // 各オイラー角を求めるのに回転行列の全成分は要らないので、一部だけ計算する。
ojan 20:00afba164688 194
ojan 25:4c72d7420d8a 195 R_11 = r_f * b_f; // 回転行列の(1,1)成分
ojan 25:4c72d7420d8a 196 R_12 = r_f * b_l; // 回転行列の(1,2)成分
ojan 20:00afba164688 197 yaw = atan2(-R_12, R_11) * RAD_TO_DEG + MAG_DECLINATION;
ojan 25:4c72d7420d8a 198 r_cos = g.GetPerpCompTo(b_f).Normalize() * b_u;
ojan 25:4c72d7420d8a 199 r_sin = Cross(g.GetPerpCompTo(b_f).Normalize(), b_u) * b_f;
ojan 25:4c72d7420d8a 200 if(r_sin > 0.0f) {
ojan 25:4c72d7420d8a 201 roll = acos(r_cos) * RAD_TO_DEG;
ojan 25:4c72d7420d8a 202 } else {
ojan 25:4c72d7420d8a 203 roll = -acos(r_cos) * RAD_TO_DEG;
ojan 25:4c72d7420d8a 204 }
YusukeWakuta 39:9ca388ce902f 205 //↓こんなメンバ関数に二個連続呼び出しみたいな書き方知らん
ojan 25:4c72d7420d8a 206 p_cos = g.GetPerpCompTo(b_l).Normalize() * b_u;
ojan 25:4c72d7420d8a 207 p_sin = Cross(g.GetPerpCompTo(b_l).Normalize(), b_u) * b_l;
ojan 25:4c72d7420d8a 208 if(p_sin > 0.0f) {
ojan 25:4c72d7420d8a 209 pitch = acos(p_cos) * RAD_TO_DEG;
ojan 25:4c72d7420d8a 210 } else {
ojan 25:4c72d7420d8a 211 pitch = -acos(p_cos) * RAD_TO_DEG;
ojan 25:4c72d7420d8a 212 }
ojan 20:00afba164688 213
ojan 20:00afba164688 214 if(yaw > 180.0f) yaw -= 360.0f; // ヨー角を[-π, π]に補正
ojan 30:fb310564097b 215 else if(yaw < -180.0f) yaw += 360.0f; // ヨー角を[-π, π]に補正
YusukeWakuta 38:ada39f1c6c76 216
YusukeWakuta 38:ada39f1c6c76 217 //pc.printf("roll:%7f,pitch:%7f,yaw:%7f\n\r",roll,pitch,yaw);
ojan 20:00afba164688 218 }
ojan 20:00afba164688 219
ojan 20:00afba164688 220
ojan 23:79cdc1432160 221 /** 各種センサーの初期化を行う関数
ojan 17:03b45055ca05 222 *
ojan 17:03b45055ca05 223 */
ojan 23:79cdc1432160 224 void SensorsInit()
ojan 20:00afba164688 225 {
taurin 36:94dc027e05cd 226
ojan 23:79cdc1432160 227 if(!mpu.init()) error("mpu6050 Initialize Error !!"); // mpu6050初期化
YusukeWakuta 38:ada39f1c6c76 228 //if(!hmc.init()) error("hmc5883l Initialize Error !!"); // hmc5883l初期化
taurin 36:94dc027e05cd 229
ojan 17:03b45055ca05 230 //重力ベクトルの初期化
ojan 17:03b45055ca05 231 raw_g.SetComp(1, 0.0f);
ojan 17:03b45055ca05 232 raw_g.SetComp(2, 0.0f);
ojan 17:03b45055ca05 233 raw_g.SetComp(3, 1.0f);
ojan 17:03b45055ca05 234
ojan 17:03b45055ca05 235 // 機体に固定されたベクトルの初期化
ojan 17:03b45055ca05 236 b_f.SetComp(1, 0.0f);
ojan 17:03b45055ca05 237 b_f.SetComp(2, 0.0f);
ojan 33:56163d4e2c53 238 b_f.SetComp(3, -1.0f);
ojan 31:2f88240999fe 239 b_u.SetComp(1, 1.0f);
ojan 17:03b45055ca05 240 b_u.SetComp(2, 0.0f);
ojan 17:03b45055ca05 241 b_u.SetComp(3, 0.0f);
ojan 17:03b45055ca05 242 b_l = Cross(b_u, b_f);
ojan 20:00afba164688 243
ojan 17:03b45055ca05 244 // 目標座標をベクトルに代入
YusukeWakuta 38:ada39f1c6c76 245 // target_p.SetComp(1, target_x * DEG_TO_RAD);
YusukeWakuta 38:ada39f1c6c76 246 // target_p.SetComp(2, target_y * DEG_TO_RAD);
ojan 17:03b45055ca05 247 }
ojan 17:03b45055ca05 248
ojan 15:d14d385d37e2 249 void KalmanInit()
ojan 15:d14d385d37e2 250 {
ojan 11:083c8c9a5b84 251 // 重力
ojan 11:083c8c9a5b84 252 {
ojan 11:083c8c9a5b84 253 // 誤差共分散行列の値を決める（対角成分のみ）
ojan 13:df1e8a650185 254 float alpha_R2 = 0.002f;
ojan 12:0d978eb4d639 255 float alpha_Q2 = 0.5f;
ojan 11:083c8c9a5b84 256 R2 *= alpha_R2;
ojan 11:083c8c9a5b84 257 Q2 *= alpha_Q2;
ojan 15:d14d385d37e2 258
ojan 13:df1e8a650185 259 // 観測方程式のヤコビアンの値を設定（時間変化無し）
ojan 13:df1e8a650185 260 float h2[15] = {
ojan 13:df1e8a650185 261 1.0f, 0.0f, 0.0f, 0.0f, 0.0f,
ojan 15:d14d385d37e2 262 0.0f, 1.0f, 0.0f, 0.0f, 0.0f,
ojan 13:df1e8a650185 263 0.0f, 0.0f, 1.0f, 0.0f, 0.0f
ojan 11:083c8c9a5b84 264 };
ojan 15:d14d385d37e2 265
ojan 13:df1e8a650185 266 H2.SetComps(h2);
ojan 11:083c8c9a5b84 267 }
ojan 15:d14d385d37e2 268
ojan 11:083c8c9a5b84 269 // 地磁気
ojan 11:083c8c9a5b84 270 {
ojan 11:083c8c9a5b84 271 // 誤差共分散行列の値を決める（対角成分のみ）
ojan 13:df1e8a650185 272 float alpha_R1 = 0.002f;
ojan 13:df1e8a650185 273 float alpha_Q1 = 0.5f;
ojan 11:083c8c9a5b84 274 R1 *= alpha_R1;
ojan 11:083c8c9a5b84 275 Q1 *= alpha_Q1;
ojan 15:d14d385d37e2 276
ojan 13:df1e8a650185 277 // 観測方程式のヤコビアンの値を設定（時間変化無し）
ojan 13:df1e8a650185 278 float h1[21] = {
ojan 13:df1e8a650185 279 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f,
ojan 13:df1e8a650185 280 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
ojan 13:df1e8a650185 281 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f
ojan 11:083c8c9a5b84 282 };
ojan 15:d14d385d37e2 283
ojan 13:df1e8a650185 284 H1.SetComps(h1);
ojan 11:083c8c9a5b84 285 }
ojan 3:5358a691a100 286 }
ojan 3:5358a691a100 287
ojan 17:03b45055ca05 288 /** カルマンフィルタの更新を行う関数
ojan 17:03b45055ca05 289 *
ojan 17:03b45055ca05 290 */
ojan 15:d14d385d37e2 291 void KalmanUpdate()
ojan 15:d14d385d37e2 292 {
ojan 13:df1e8a650185 293 // 重力
ojan 15:d14d385d37e2 294
ojan 11:083c8c9a5b84 295 {
ojan 13:df1e8a650185 296 // ヤコビアンの更新
ojan 13:df1e8a650185 297 float f2[25] = {
ojan 15:d14d385d37e2 298 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,
ojan 15:d14d385d37e2 299 -(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,
ojan 15:d14d385d37e2 300 (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,
ojan 15:d14d385d37e2 301 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,
ojan 13:df1e8a650185 302 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
ojan 13:df1e8a650185 303 };
ojan 15:d14d385d37e2 304
ojan 13:df1e8a650185 305 F2.SetComps(f2);
ojan 15:d14d385d37e2 306
ojan 11:083c8c9a5b84 307 // 事前推定値の更新
ojan 13:df1e8a650185 308 //pri_x2 = F2 * post_x2;
ojan 15:d14d385d37e2 309
ojan 13:df1e8a650185 310 float pri_x2_vals[5] = {
ojan 15:d14d385d37e2 311 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,
ojan 15:d14d385d37e2 312 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,
ojan 15:d14d385d37e2 313 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,
ojan 15:d14d385d37e2 314 post_x2.GetComp(4),
ojan 13:df1e8a650185 315 post_x2.GetComp(5)
ojan 13:df1e8a650185 316 };
ojan 15:d14d385d37e2 317
ojan 13:df1e8a650185 318 pri_x2.SetComps(pri_x2_vals);
ojan 15:d14d385d37e2 319
ojan 11:083c8c9a5b84 320 // 事前誤差分散行列の更新
ojan 11:083c8c9a5b84 321 pri_P2 = F2 * post_P2 * F2.Transpose() + R2;
ojan 15:d14d385d37e2 322
ojan 11:083c8c9a5b84 323 // カルマンゲインの計算
ojan 11:083c8c9a5b84 324 S2 = Q2 + H2 * pri_P2 * H2.Transpose();
ojan 13:df1e8a650185 325 static float det;
ojan 11:083c8c9a5b84 326 if((det = S2.Inverse(S_inv2)) >= 0.0f) {
ojan 11:083c8c9a5b84 327 pc.printf("E:%.3f\r\n", det);
ojan 11:083c8c9a5b84 328 return; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了
ojan 11:083c8c9a5b84 329 }
ojan 15:d14d385d37e2 330 K2 = pri_P2 * H2.Transpose() * S_inv2;
ojan 15:d14d385d37e2 331
ojan 11:083c8c9a5b84 332 // 事後推定値の更新
ojan 13:df1e8a650185 333 post_x2 = pri_x2 + K2 * (raw_acc - H2 * pri_x2);
ojan 11:083c8c9a5b84 334 // 事後誤差分散行列の更新
ojan 11:083c8c9a5b84 335 post_P2 = (I2 - K2 * H2) * pri_P2;
ojan 11:083c8c9a5b84 336 }
ojan 15:d14d385d37e2 337
ojan 15:d14d385d37e2 338
ojan 11:083c8c9a5b84 339 // 地磁気
ojan 11:083c8c9a5b84 340 {
ojan 11:083c8c9a5b84 341 // ヤコビアンの更新
ojan 13:df1e8a650185 342 float f1[49] = {
ojan 15:d14d385d37e2 343 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,
ojan 15:d14d385d37e2 344 -(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,
ojan 15:d14d385d37e2 345 (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,
ojan 15:d14d385d37e2 346 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f,
ojan 15:d14d385d37e2 347 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
ojan 15:d14d385d37e2 348 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,
ojan 13:df1e8a650185 349 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
ojan 11:083c8c9a5b84 350 };
ojan 15:d14d385d37e2 351
ojan 13:df1e8a650185 352 F1.SetComps(f1);
ojan 15:d14d385d37e2 353
ojan 11:083c8c9a5b84 354 // 事前推定値の更新
ojan 13:df1e8a650185 355 //pri_x1 = F1 * post_x1;
ojan 13:df1e8a650185 356 float pri_x1_vals[7] = {
ojan 15:d14d385d37e2 357 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,
ojan 15:d14d385d37e2 358 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,
ojan 15:d14d385d37e2 359 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,
ojan 15:d14d385d37e2 360 post_x1.GetComp(4),
ojan 15:d14d385d37e2 361 post_x1.GetComp(5),
ojan 15:d14d385d37e2 362 post_x1.GetComp(6),
ojan 13:df1e8a650185 363 post_x1.GetComp(7)
ojan 13:df1e8a650185 364 };
ojan 15:d14d385d37e2 365
ojan 13:df1e8a650185 366 pri_x1.SetComps(pri_x1_vals);
ojan 15:d14d385d37e2 367
ojan 11:083c8c9a5b84 368 // 事前誤差分散行列の更新
ojan 11:083c8c9a5b84 369 pri_P1 = F1 * post_P1 * F1.Transpose() + R1;
ojan 15:d14d385d37e2 370
ojan 11:083c8c9a5b84 371 // カルマンゲインの計算
ojan 11:083c8c9a5b84 372 S1 = Q1 + H1 * pri_P1 * H1.Transpose();
ojan 13:df1e8a650185 373 static float det;
ojan 11:083c8c9a5b84 374 if((det = S1.Inverse(S_inv1)) >= 0.0f) {
ojan 11:083c8c9a5b84 375 pc.printf("E:%.3f\r\n", det);
ojan 11:083c8c9a5b84 376 return; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了
ojan 11:083c8c9a5b84 377 }
ojan 15:d14d385d37e2 378 K1 = pri_P1 * H1.Transpose() * S_inv1;
ojan 15:d14d385d37e2 379
ojan 11:083c8c9a5b84 380 // 事後推定値の更新
ojan 11:083c8c9a5b84 381 post_x1 = pri_x1 + K1 * (raw_geomag - H1 * pri_x1);
ojan 11:083c8c9a5b84 382 // 事後誤差分散行列の更新
ojan 11:083c8c9a5b84 383 post_P1 = (I1 - K1 * H1) * pri_P1;
ojan 3:5358a691a100 384 }
ojan 3:5358a691a100 385 }
ojan 3:5358a691a100 386
ojan 23:79cdc1432160 387 /* ------------------------------ 割り込み関数 ------------------------------ */
ojan 23:79cdc1432160 388
ojan 20:00afba164688 389 /** データ取得＆更新関数
ojan 20:00afba164688 390 *
ojan 20:00afba164688 391 */
ojan 20:00afba164688 392 void DataUpdate()
ojan 15:d14d385d37e2 393 {
ojan 4:45dc5590abc0 394 // センサーの値を更新
ojan 4:45dc5590abc0 395 mpu.read();
YusukeWakuta 38:ada39f1c6c76 396 // hmc.read();
YusukeWakuta 39:9ca388ce902f 397 /*//↓i2cの通信に成功しているかどうかを確認（w）
YusukeWakuta 38:ada39f1c6c76 398 //===========================================================
YusukeWakuta 38:ada39f1c6c76 399 if(mpu.checker_get() == 0){
YusukeWakuta 38:ada39f1c6c76 400 pc.printf("SUCCESS!!\n\r");
YusukeWakuta 38:ada39f1c6c76 401 }
YusukeWakuta 38:ada39f1c6c76 402 for(i = 0;i < 13;i++){
YusukeWakuta 38:ada39f1c6c76 403 pc.printf("%u\n\r",mpu.data.reg[i]);
YusukeWakuta 38:ada39f1c6c76 404 }
YusukeWakuta 38:ada39f1c6c76 405 //============================================================
YusukeWakuta 39:9ca388ce902f 406 */
ojan 4:45dc5590abc0 407 for(int i=0; i<3; i++) {
ojan 4:45dc5590abc0 408 raw_acc.SetComp(i+1, (float)mpu.data.value.acc[i] * ACC_LSB_TO_G);
ojan 4:45dc5590abc0 409 raw_gyro.SetComp(i+1, (float)mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD);
YusukeWakuta 38:ada39f1c6c76 410 // raw_geomag.SetComp(i+1, (float)hmc.data.value[i] * MAG_LSB_TO_GAUSS);
ojan 4:45dc5590abc0 411 }
ojan 15:d14d385d37e2 412
ojan 14:f85cb5340cb8 413 Vector delta_g = Cross(raw_gyro, raw_g); // Δg = ω × g
ojan 4:45dc5590abc0 414 raw_g = 0.9f * (raw_g - delta_g * dt) + 0.1f * raw_acc.Normalize(); // 相補フィルタ
ojan 4:45dc5590abc0 415 raw_g = raw_g.Normalize();
ojan 15:d14d385d37e2 416
ojan 4:45dc5590abc0 417 KalmanUpdate();
ojan 15:d14d385d37e2 418
ojan 24:8838be99cec3 419 if(INT_flag) {
ojan 4:45dc5590abc0 420 g = raw_g;
ojan 4:45dc5590abc0 421 for(int i=0; i<3; i++) {
ojan 11:083c8c9a5b84 422 geomag.SetComp(i+1, post_x1.GetComp(i+1));
ojan 4:45dc5590abc0 423 }
ojan 4:45dc5590abc0 424 acc = raw_acc;
ojan 4:45dc5590abc0 425 gyro = raw_gyro;
ojan 4:45dc5590abc0 426 press = raw_press;
ojan 23:79cdc1432160 427 temp = raw_temp;
ojan 15:d14d385d37e2 428
ojan 13:df1e8a650185 429 vrt_acc = raw_acc * raw_g;
ojan 0:bc6f14fc60c7 430 }
ojan 3:5358a691a100 431 }