albatross / Mbed 2 deprecated LAURUS_program_copy

MPUとHMCでうごくかもver

Dependencies:   ConfigFile SDFileSystem mbed

Fork of LAURUS_program by hiroya taura

main.cpp@43:3a37e39b234c, 2015-12-26 (annotated)

Committer:
YusukeWakuta
Date:
Sat Dec 26 11:44:09 2015 +0000
Revision:
43:3a37e39b234c
Parent:
42:7428acb9b14d 
hmc??????????????????????????????????????????

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