hiroya taura / Mbed 2 deprecated LAURUS_program

MPU6050のサンプルプログラム２

Dependencies:   ConfigFile SDFileSystem mbed

Fork of LAURUS_program by LAURUS

main.cpp@9:6d4578dcc1ed, 2015-06-15 (annotated)

Committer:
ojan
Date:
Mon Jun 15 10:40:18 2015 +0000
Revision:
9:6d4578dcc1ed
Parent:
8:602865d8fca3
Child:
10:8ee11e412ad7
add code to calculate Euler angle

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"
ojan 0:bc6f14fc60c7 3 #include "HMC5883L.h"
ojan 0:bc6f14fc60c7 4 #include "LPS25H.h"
ojan 1:6cd6d2760856 5 #include "GMS6_CR6.h"
ojan 0:bc6f14fc60c7 6 #include "ErrorLogger.h"
ojan 0:bc6f14fc60c7 7 #include "Vector.h"
ojan 3:5358a691a100 8 #include "Matrix.h"
ojan 3:5358a691a100 9 #include "Vector_Matrix_operator.h"
ojan 0:bc6f14fc60c7 10 #include "myConstants.h"
ojan 3:5358a691a100 11 #include "Log.h"
ojan 0:bc6f14fc60c7 12
ojan 0:bc6f14fc60c7 13 /********** private define **********/
ojan 0:bc6f14fc60c7 14 #define TRUE 1
ojan 0:bc6f14fc60c7 15 #define FALSE 0
ojan 8:602865d8fca3 16
ojan 9:6d4578dcc1ed 17 #define x 1
ojan 9:6d4578dcc1ed 18 #define y 2
ojan 9:6d4578dcc1ed 19 #define z 3
ojan 9:6d4578dcc1ed 20
ojan 8:602865d8fca3 21 const float dt = 0.1f; // 割り込み周期(s)
ojan 8:602865d8fca3 22 const float ServoMax = 0.0023f; // サーボの最大パルス長(s)
ojan 8:602865d8fca3 23 const float ServoMin = 0.0006f; // サーボの最小パルス長(s)
ojan 9:6d4578dcc1ed 24 const float PullMax = 25.0f; // 引っ張れる紐の最大量(mm)
ojan 0:bc6f14fc60c7 25 /********** private macro **********/
ojan 1:6cd6d2760856 26
ojan 0:bc6f14fc60c7 27 /********** private typedef **********/
ojan 1:6cd6d2760856 28
ojan 0:bc6f14fc60c7 29 /********** private variables **********/
ojan 4:45dc5590abc0 30 DigitalOut myled(LED1); // デバッグ用LEDのためのデジタル出力
ojan 4:45dc5590abc0 31 I2C i2c(PB_9, PB_8); // I2Cポート
ojan 1:6cd6d2760856 32 MPU6050 mpu(&i2c); // 加速度・角速度センサ
ojan 1:6cd6d2760856 33 HMC5883L hmc(&i2c); // 地磁気センサ
ojan 1:6cd6d2760856 34 LPS25H lps(&i2c); // 気圧センサ
ojan 8:602865d8fca3 35 Serial gps(PA_11, PA_12); // GPS通信用シリアルポート
ojan 1:6cd6d2760856 36 Serial pc(SERIAL_TX, SERIAL_RX); // PC通信用シリアルポート
ojan 1:6cd6d2760856 37 GMS6_CR6 gms(&gps, &pc); // GPS
onaka 6:2b68f85a984a 38 Log logger(PB_5, PB_4, PB_3, PB_10, PA_9, PA_10, PC_1); // ロガー（microSD、XBee）
ojan 8:602865d8fca3 39 PwmOut servoL(PB_6), servoR(PC_7); // サーボ用PWM出力
ojan 8:602865d8fca3 40 AnalogIn rf(PC_0); // 照度センサ用アナログ入力
ojan 8:602865d8fca3 41 AnalogIn servoVcc(PA_0); // バッテリー電圧監視用アナログ入力(サーボ用)
ojan 8:602865d8fca3 42 AnalogIn logicVcc(PA_1); // バッテリー電圧監視用アナログ入力(ロジック用)
ojan 8:602865d8fca3 43 Ticker INT_timer; // 割り込みタイマー
ojan 0:bc6f14fc60c7 44
ojan 8:602865d8fca3 45 int lps_cnt = 0; // 気圧センサ読み取りカウント
ojan 8:602865d8fca3 46 uint8_t INT_flag = TRUE; // 割り込み可否フラグ
ojan 8:602865d8fca3 47 Vector raw_acc(3); // 加速度(m/s^2) 生
ojan 8:602865d8fca3 48 Vector raw_gyro(3); // 角速度(deg/s) 生
ojan 8:602865d8fca3 49 Vector raw_geomag(3); // 地磁気(?) 生
ojan 8:602865d8fca3 50 float raw_press; // 気圧(hPa) 生
ojan 8:602865d8fca3 51 Vector acc(3); // 加速度(m/s^2)
ojan 8:602865d8fca3 52 Vector gyro(3); // 角速度(deg/s)
ojan 8:602865d8fca3 53 Vector geomag(3); // 地磁気(?)
ojan 8:602865d8fca3 54 float press; // 気圧(hPa)
ojan 1:6cd6d2760856 55
ojan 8:602865d8fca3 56 Vector raw_g(3); // 重力ベクトル 生
ojan 8:602865d8fca3 57 Vector g(3); // 重力ベクトル
ojan 9:6d4578dcc1ed 58 Vector p(2); // 位置情報(経度, 緯度)
ojan 9:6d4578dcc1ed 59 Vector pre_p(2); // 過去の位置情報(経度, 緯度)
ojan 9:6d4578dcc1ed 60 int UTC_t = 0; // UTC時刻
ojan 9:6d4578dcc1ed 61 int pre_UTC_t = 0; // 前のUTC時刻
ojan 8:602865d8fca3 62 //Vector n(3); // 地磁気ベクトル
ojan 8:602865d8fca3 63
ojan 9:6d4578dcc1ed 64 Vector b_f(3); // 機体座標に固定された、機体前方向きのベクトル（x軸）
ojan 9:6d4578dcc1ed 65 Vector b_u(3); // 機体座標に固定された、機体上方向きのベクトル（z軸）
ojan 9:6d4578dcc1ed 66 Vector b_l(3); // 機体座標に固定された、機体左方向きのベクトル（y軸）
ojan 9:6d4578dcc1ed 67
ojan 9:6d4578dcc1ed 68 Vector r_f(3); // 参照座標に固定された、北向きのベクトル（X軸）
ojan 9:6d4578dcc1ed 69 Vector r_u(3); // 参照座標に固定された、上向きのベクトル（Z軸）
ojan 9:6d4578dcc1ed 70 Vector r_l(3); // 参照座標に固定された、西向きのベクトル（Y軸）
ojan 9:6d4578dcc1ed 71
ojan 9:6d4578dcc1ed 72 int pull_L = 0; // 左サーボの引っ張り量(mm：0~PullMax)
ojan 9:6d4578dcc1ed 73 int pull_R = 0; // 右サーボの引っ張り量(mm：0~PullMax)
ojan 9:6d4578dcc1ed 74
ojan 9:6d4578dcc1ed 75 float yaw = 0.0f; // 本体のヨー角（deg）
ojan 9:6d4578dcc1ed 76 float pitch = 0.0f; // 本体のピッチ角（deg）
ojan 9:6d4578dcc1ed 77 float roll = 0.0f; // 本体のロール角（deg）
ojan 3:5358a691a100 78
ojan 3:5358a691a100 79 /* ----- Kalman Filter ----- */
ojan 3:5358a691a100 80 Vector pri_x(6);
ojan 3:5358a691a100 81 Matrix pri_P(6, 6);
ojan 3:5358a691a100 82 Vector post_x(6);
ojan 3:5358a691a100 83 Matrix post_P(6, 6);
ojan 3:5358a691a100 84 Matrix F(6, 6), H(3, 6);
ojan 3:5358a691a100 85 Matrix R(6, 6), Q(3, 3);
ojan 3:5358a691a100 86 Matrix I(6, 6);
ojan 3:5358a691a100 87 Matrix K(6, 3);
ojan 3:5358a691a100 88 Matrix S(3, 3), inv(3, 3);
ojan 3:5358a691a100 89 /* ----- ------------- ----- */
ojan 3:5358a691a100 90
ojan 3:5358a691a100 91 Timer timer;
ojan 3:5358a691a100 92
onaka 6:2b68f85a984a 93 char data[512] = {};
ojan 1:6cd6d2760856 94
ojan 0:bc6f14fc60c7 95 /********** private functions **********/
ojan 9:6d4578dcc1ed 96 void KalmanInit(); // カルマンフィルタ初期化
ojan 9:6d4578dcc1ed 97 void KalmanUpdate(); // カルマンフィルタ更新
ojan 9:6d4578dcc1ed 98 float distance(Vector p1, Vector p2); // 緯度・経度の差から2点間の距離を算出(m)
ojan 9:6d4578dcc1ed 99 void INT_func(); // 割り込み用関数
ojan 3:5358a691a100 100 void toString(Matrix& m);
ojan 3:5358a691a100 101 void toString(Vector& v);
ojan 1:6cd6d2760856 102
ojan 0:bc6f14fc60c7 103 /********** main function **********/
ojan 0:bc6f14fc60c7 104
ojan 0:bc6f14fc60c7 105 int main() {
ojan 0:bc6f14fc60c7 106
ojan 0:bc6f14fc60c7 107 i2c.frequency(400000); // I2Cの通信速度を400kHzに設定
ojan 0:bc6f14fc60c7 108
ojan 0:bc6f14fc60c7 109 if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!"); // mpu6050初期化
ojan 0:bc6f14fc60c7 110 if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!"); // hmc5883l初期化
ojan 0:bc6f14fc60c7 111
ojan 4:45dc5590abc0 112 char* title = "log data\r\n"; // ログのタイトル行
onaka 6:2b68f85a984a 113 logger.initialize_sdlog(title); // ログファイル初期設定
ojan 3:5358a691a100 114
ojan 8:602865d8fca3 115 servoL.period(0.020f); // サーボの信号の周期は20ms
ojan 8:602865d8fca3 116 servoR.period(0.020f);
ojan 8:602865d8fca3 117
ojan 3:5358a691a100 118 KalmanInit();
ojan 3:5358a691a100 119
ojan 3:5358a691a100 120 INT_timer.attach(&INT_func, dt); // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
ojan 1:6cd6d2760856 121
ojan 1:6cd6d2760856 122 //重力ベクトルの初期化
ojan 4:45dc5590abc0 123 raw_g.SetComp(1, 0.0f);
ojan 4:45dc5590abc0 124 raw_g.SetComp(2, 0.0f);
ojan 4:45dc5590abc0 125 raw_g.SetComp(3, 1.0f);
ojan 0:bc6f14fc60c7 126
ojan 9:6d4578dcc1ed 127 // 機体に固定されたベクトルの初期化
ojan 9:6d4578dcc1ed 128 b_f.SetComp(1, 0.0f);
ojan 9:6d4578dcc1ed 129 b_f.SetComp(2, 0.0f);
ojan 9:6d4578dcc1ed 130 b_f.SetComp(3, -1.0f);
ojan 9:6d4578dcc1ed 131 b_u.SetComp(1, 1.0f);
ojan 9:6d4578dcc1ed 132 b_u.SetComp(2, 0.0f);
ojan 9:6d4578dcc1ed 133 b_u.SetComp(3, 0.0f);
ojan 9:6d4578dcc1ed 134 b_l = Cross(b_u, b_f);
ojan 9:6d4578dcc1ed 135
ojan 2:d2b60a1d0cd9 136 /* ---------- ↓↓↓ ここからメインループ ↓↓↓ ---------- */
ojan 0:bc6f14fc60c7 137 while(1) {
ojan 4:45dc5590abc0 138 timer.stop();
ojan 4:45dc5590abc0 139 timer.reset();
ojan 4:45dc5590abc0 140 timer.start();
ojan 3:5358a691a100 141 // 0.1秒おきにセンサーの出力をpcへ出力
ojan 4:45dc5590abc0 142 myled = 1; // LED is ON
ojan 8:602865d8fca3 143
ojan 0:bc6f14fc60c7 144 INT_flag = FALSE; // 割り込みによる変数書き換えを阻止
ojan 0:bc6f14fc60c7 145
ojan 9:6d4578dcc1ed 146 // データ処理
ojan 9:6d4578dcc1ed 147 {
ojan 9:6d4578dcc1ed 148 gms.read(); // GPSデータ取得
ojan 9:6d4578dcc1ed 149 UTC_t = (int)gms.time;
ojan 9:6d4578dcc1ed 150
ojan 9:6d4578dcc1ed 151 // 参照座標系の基底を求める
ojan 9:6d4578dcc1ed 152 r_u = g;
ojan 9:6d4578dcc1ed 153 r_f = geomag.GetPerpCompTo(g).Normalize();
ojan 9:6d4578dcc1ed 154 r_l = Cross(r_u, r_f);
ojan 9:6d4578dcc1ed 155
ojan 9:6d4578dcc1ed 156 // 回転行列を経由してオイラー角を求める
ojan 9:6d4578dcc1ed 157 // オイラー角はヨー・ピッチ・ロールの順に回転したものとする
ojan 9:6d4578dcc1ed 158 // 各オイラー角を求めるのに回転行列の全成分は要らないので、一部だけ計算する。
ojan 9:6d4578dcc1ed 159
ojan 9:6d4578dcc1ed 160 float R_11 = r_f * b_f; // 回転行列の(1,1)成分
ojan 9:6d4578dcc1ed 161 float R_12 = r_f * b_l; // 回転行列の(1,2)成分
ojan 9:6d4578dcc1ed 162 float R_13 = r_f * b_u; // 回転行列の(1,3)成分
ojan 9:6d4578dcc1ed 163 float R_23 = r_l * b_u; // 回転行列の(2,3)成分
ojan 9:6d4578dcc1ed 164 float R_33 = r_u * b_u; // 回転行列の(3,3)成分
ojan 9:6d4578dcc1ed 165
ojan 9:6d4578dcc1ed 166 yaw = atan2(R_11, -R_12) * RAD_TO_DEG;
ojan 9:6d4578dcc1ed 167 roll = atan2(R_33, -R_23) * RAD_TO_DEG;
ojan 9:6d4578dcc1ed 168 pitch = atan2(R_11/cos(yaw), R_13) * RAD_TO_DEG;
ojan 9:6d4578dcc1ed 169
ojan 9:6d4578dcc1ed 170 if(UTC_t - pre_UTC_t >= 1) { // GPSデータが更新されていたら
ojan 9:6d4578dcc1ed 171 p.SetComp(1, gms.longitude * DEG_TO_RAD);
ojan 9:6d4578dcc1ed 172 p.SetComp(2, gms.latitude * DEG_TO_RAD);
ojan 9:6d4578dcc1ed 173 } else { // 更新されていなかったら
ojan 9:6d4578dcc1ed 174
ojan 9:6d4578dcc1ed 175 }
ojan 9:6d4578dcc1ed 176
ojan 9:6d4578dcc1ed 177 pre_p = p;
ojan 9:6d4578dcc1ed 178 pre_UTC_t = UTC_t;
ojan 9:6d4578dcc1ed 179 }
ojan 9:6d4578dcc1ed 180
ojan 5:182f6356bce1 181 float sv = (float)servoVcc.read_u16() * ADC_LSB_TO_V * 2.0f;
ojan 5:182f6356bce1 182 float lv = (float)logicVcc.read_u16() * ADC_LSB_TO_V * 2.0f;
ojan 5:182f6356bce1 183
ojan 5:182f6356bce1 184 sprintf(data, "%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%d\r\n",
ojan 5:182f6356bce1 185 g.GetComp(1), g.GetComp(2), g.GetComp(3),
ojan 4:45dc5590abc0 186 geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3),
ojan 5:182f6356bce1 187 press, gms.longitude, gms.latitude,
ojan 8:602865d8fca3 188 sv, lv, rf.read_u16());
onaka 6:2b68f85a984a 189 logger.write(data);
ojan 4:45dc5590abc0 190
ojan 0:bc6f14fc60c7 191 INT_flag = TRUE; // 割り込み許可
ojan 0:bc6f14fc60c7 192
ojan 5:182f6356bce1 193 // 制御ルーチン
ojan 9:6d4578dcc1ed 194 {
ojan 9:6d4578dcc1ed 195 //pull_L = (pull_L+5)%30;
ojan 9:6d4578dcc1ed 196 //pull_R = (pull_R+5)%30;
ojan 9:6d4578dcc1ed 197 pull_L = 0;
ojan 9:6d4578dcc1ed 198 pull_R = 30;
ojan 9:6d4578dcc1ed 199 servoL.pulsewidth((ServoMax - ServoMin) * pull_L / PullMax + ServoMin);
ojan 9:6d4578dcc1ed 200 servoR.pulsewidth((ServoMax - ServoMin) * pull_R / PullMax + ServoMin);
ojan 8:602865d8fca3 201
ojan 9:6d4578dcc1ed 202 }
ojan 9:6d4578dcc1ed 203
ojan 9:6d4578dcc1ed 204 myled = 0; // LED is OFF
ojan 5:182f6356bce1 205
ojan 9:6d4578dcc1ed 206 // ループはきっかり0.2秒ごと
ojan 9:6d4578dcc1ed 207 while(timer.read_ms() < 200);
ojan 5:182f6356bce1 208
ojan 9:6d4578dcc1ed 209 pc.printf("time: %.3f[s]\r\n", (float)timer.read_ms()/1000.0f);
ojan 4:45dc5590abc0 210
ojan 0:bc6f14fc60c7 211 }
ojan 2:d2b60a1d0cd9 212
ojan 2:d2b60a1d0cd9 213 /* ---------- ↑↑↑ ここまでメインループ ↑↑↑ ---------- */
ojan 0:bc6f14fc60c7 214 }
ojan 0:bc6f14fc60c7 215
ojan 3:5358a691a100 216 void KalmanInit() {
ojan 3:5358a691a100 217
ojan 3:5358a691a100 218 // 誤差共分散行列の値を決める（対角成分のみ）
ojan 3:5358a691a100 219 float alpha_R = 60.0f;
ojan 3:5358a691a100 220 float alpha_Q = 100.0f;
ojan 3:5358a691a100 221 R *= alpha_R;
ojan 3:5358a691a100 222 Q *= alpha_Q;
ojan 3:5358a691a100 223
ojan 3:5358a691a100 224 // 状態方程式のヤコビアンの初期値を代入（時間変化あり）
ojan 3:5358a691a100 225 float f[36] = {
ojan 4:45dc5590abc0 226 1.0f, raw_gyro.GetComp(3)*dt, -raw_gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f,
ojan 4:45dc5590abc0 227 -raw_gyro.GetComp(3)*dt, 1.0f, raw_gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f,
ojan 4:45dc5590abc0 228 raw_gyro.GetComp(2)*dt, -raw_gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f,
ojan 3:5358a691a100 229 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
ojan 3:5358a691a100 230 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,
ojan 3:5358a691a100 231 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
ojan 3:5358a691a100 232 };
ojan 3:5358a691a100 233
ojan 3:5358a691a100 234 F.SetComps(f);
ojan 3:5358a691a100 235
ojan 3:5358a691a100 236 // 観測方程式のヤコビアンの値を設定（時間変化無し）
ojan 3:5358a691a100 237 float h[18] = {
ojan 3:5358a691a100 238 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
ojan 3:5358a691a100 239 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f,
ojan 3:5358a691a100 240 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f
ojan 3:5358a691a100 241 };
ojan 3:5358a691a100 242
ojan 3:5358a691a100 243 H.SetComps(h);
ojan 3:5358a691a100 244 }
ojan 3:5358a691a100 245
ojan 3:5358a691a100 246 void KalmanUpdate() {
ojan 3:5358a691a100 247 // ヤコビアンの更新
ojan 3:5358a691a100 248 float f[36] = {
ojan 4:45dc5590abc0 249 1.0f, raw_gyro.GetComp(3)*dt, -raw_gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f,
ojan 4:45dc5590abc0 250 -raw_gyro.GetComp(3)*dt, 1.0f, raw_gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f,
ojan 4:45dc5590abc0 251 raw_gyro.GetComp(2)*dt, -raw_gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f,
ojan 3:5358a691a100 252 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
ojan 3:5358a691a100 253 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,
ojan 3:5358a691a100 254 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
ojan 3:5358a691a100 255 };
ojan 3:5358a691a100 256
ojan 3:5358a691a100 257 F.SetComps(f);
ojan 3:5358a691a100 258
ojan 3:5358a691a100 259 // 事前推定値の更新
ojan 3:5358a691a100 260 pri_x = F * post_x;
ojan 3:5358a691a100 261 // 事前誤差分散行列の更新
ojan 3:5358a691a100 262 pri_P = F * post_P * F.Transpose() + R;
ojan 3:5358a691a100 263
ojan 3:5358a691a100 264 // カルマンゲインの計算
ojan 3:5358a691a100 265 S = Q + H * pri_P * H.Transpose();
ojan 3:5358a691a100 266 float det;
ojan 3:5358a691a100 267 if((det = S.Inverse(inv)) >= 0.0f) {
ojan 3:5358a691a100 268 pc.printf("E:%.3f\r\n", det);
ojan 3:5358a691a100 269 return; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了
ojan 3:5358a691a100 270 }
ojan 3:5358a691a100 271 K = pri_P * H.Transpose() * inv;
ojan 3:5358a691a100 272
ojan 3:5358a691a100 273 // 事後推定値の更新
ojan 4:45dc5590abc0 274 post_x = pri_x + K * (raw_geomag - H * pri_x);
ojan 3:5358a691a100 275 // 事後誤差分散行列の更新
ojan 3:5358a691a100 276 post_P = (I - K * H) * pri_P;
ojan 3:5358a691a100 277 }
ojan 3:5358a691a100 278
ojan 9:6d4578dcc1ed 279 float distance(Vector p1, Vector p2) {
ojan 9:6d4578dcc1ed 280 if(p1.GetDim() != p2.GetDim()) return 0.0f;
ojan 9:6d4578dcc1ed 281
ojan 9:6d4578dcc1ed 282 float mu_y = (p1.GetComp(2) + p2.GetComp(2)) * 0.5f;
ojan 9:6d4578dcc1ed 283 float s_mu_y = sin(mu_y);
ojan 9:6d4578dcc1ed 284 float w = sqrt(1 - GPS_SQ_E * s_mu_y * s_mu_y);
ojan 9:6d4578dcc1ed 285 float m = GPS_A * (1 - GPS_SQ_E) / (w * w * w);
ojan 9:6d4578dcc1ed 286 float n = GPS_A / w;
ojan 9:6d4578dcc1ed 287 float d1 = m * (p1.GetComp(2) - p2.GetComp(2));
ojan 9:6d4578dcc1ed 288 float d2 = n * cos(mu_y) * (p1.GetComp(1) - p2.GetComp(1));
ojan 9:6d4578dcc1ed 289
ojan 9:6d4578dcc1ed 290 return sqrt(d1 * d1 + d2 * d2);
ojan 9:6d4578dcc1ed 291 }
ojan 9:6d4578dcc1ed 292
ojan 9:6d4578dcc1ed 293 /* -------------------- 割り込み関数 -------------------- */
ojan 9:6d4578dcc1ed 294
ojan 0:bc6f14fc60c7 295 void INT_func() {
ojan 4:45dc5590abc0 296 // センサーの値を更新
ojan 4:45dc5590abc0 297 mpu.read();
ojan 4:45dc5590abc0 298 hmc.read();
ojan 4:45dc5590abc0 299
ojan 4:45dc5590abc0 300 for(int i=0; i<3; i++) {
ojan 4:45dc5590abc0 301 raw_acc.SetComp(i+1, (float)mpu.data.value.acc[i] * ACC_LSB_TO_G);
ojan 4:45dc5590abc0 302 raw_gyro.SetComp(i+1, (float)mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD);
ojan 4:45dc5590abc0 303 raw_geomag.SetComp(i+1, (float)hmc.data.value[i] * MAG_LSB_TO_GAUSS);
ojan 4:45dc5590abc0 304 }
ojan 0:bc6f14fc60c7 305
ojan 4:45dc5590abc0 306 Vector delta_g = Cross(raw_gyro, raw_g); // Δg = ω × g
ojan 4:45dc5590abc0 307 raw_g = 0.9f * (raw_g - delta_g * dt) + 0.1f * raw_acc.Normalize(); // 相補フィルタ
ojan 4:45dc5590abc0 308 raw_g = raw_g.Normalize();
ojan 4:45dc5590abc0 309
ojan 4:45dc5590abc0 310 KalmanUpdate();
ojan 0:bc6f14fc60c7 311
ojan 4:45dc5590abc0 312 // LPS25Hによる気圧の取得は10Hz
ojan 4:45dc5590abc0 313 lps_cnt = (lps_cnt+1)%10;
ojan 4:45dc5590abc0 314 if(lps_cnt == 0) {
ojan 4:45dc5590abc0 315 raw_press = (float)lps.pressure() * PRES_LSB_TO_HPA;
ojan 4:45dc5590abc0 316 }
ojan 4:45dc5590abc0 317 //pc.printf("%d(us)\r\n", timer.read_us());
ojan 4:45dc5590abc0 318
ojan 4:45dc5590abc0 319 if(INT_flag != FALSE) {
ojan 4:45dc5590abc0 320 g = raw_g;
ojan 4:45dc5590abc0 321 for(int i=0; i<3; i++) {
ojan 4:45dc5590abc0 322 geomag.SetComp(i+1, post_x.GetComp(i+1));
ojan 4:45dc5590abc0 323 }
ojan 4:45dc5590abc0 324 acc = raw_acc;
ojan 4:45dc5590abc0 325 gyro = raw_gyro;
ojan 4:45dc5590abc0 326 press = raw_press;
ojan 1:6cd6d2760856 327
ojan 0:bc6f14fc60c7 328 }
ojan 3:5358a691a100 329 }
ojan 3:5358a691a100 330
ojan 9:6d4578dcc1ed 331 /* -------------------- デバッグ用関数 -------------------- */
ojan 9:6d4578dcc1ed 332
ojan 3:5358a691a100 333 void toString(Matrix& m) {
ojan 3:5358a691a100 334
ojan 3:5358a691a100 335 for(int i=0; i<m.GetRow(); i++) {
ojan 3:5358a691a100 336 for(int j=0; j<m.GetCol(); j++) {
ojan 3:5358a691a100 337 pc.printf("%.6f\t", m.GetComp(i+1, j+1));
ojan 3:5358a691a100 338 }
ojan 3:5358a691a100 339 pc.printf("\r\n");
ojan 3:5358a691a100 340 }
ojan 3:5358a691a100 341
ojan 3:5358a691a100 342 }
ojan 3:5358a691a100 343
ojan 3:5358a691a100 344 void toString(Vector& v) {
ojan 3:5358a691a100 345
ojan 3:5358a691a100 346 for(int i=0; i<v.GetDim(); i++) {
ojan 3:5358a691a100 347 pc.printf("%.6f\t", v.GetComp(i+1));
ojan 3:5358a691a100 348 }
ojan 3:5358a691a100 349 pc.printf("\r\n");
ojan 3:5358a691a100 350
ojan 0:bc6f14fc60c7 351 }