hiroya taura / Mbed 2 deprecated LAURUS_program

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

Dependencies:   ConfigFile SDFileSystem mbed

Fork of LAURUS_program by LAURUS

main.cpp@13:df1e8a650185, 2015-06-19 (annotated)

Committer:
ojan
Date:
Fri Jun 19 01:08:35 2015 +0000
Revision:
13:df1e8a650185
Parent:
12:0d978eb4d639
Child:
14:f85cb5340cb8
add scripts to avoid Spiral.; modify Kalman filter model.

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"
onaka 7:0ec343d29641 11 #include "SDFileSystem.h"
onaka 7:0ec343d29641 12 #include "BufferedSerial.h"
onaka 7:0ec343d29641 13 #include "ConfigFile.h"
ojan 0:bc6f14fc60c7 14
ojan 0:bc6f14fc60c7 15 /********** private define **********/
ojan 0:bc6f14fc60c7 16 #define TRUE 1
ojan 0:bc6f14fc60c7 17 #define FALSE 0
ojan 8:602865d8fca3 18
ojan 13:df1e8a650185 19 const float dt = 0.01f; // 割り込み周期(s)
ojan 8:602865d8fca3 20 const float ServoMax = 0.0023f; // サーボの最大パルス長(s)
ojan 8:602865d8fca3 21 const float ServoMin = 0.0006f; // サーボの最小パルス長(s)
ojan 9:6d4578dcc1ed 22 const float PullMax = 25.0f; // 引っ張れる紐の最大量(mm)
ojan 0:bc6f14fc60c7 23 /********** private macro **********/
ojan 1:6cd6d2760856 24
ojan 0:bc6f14fc60c7 25 /********** private typedef **********/
ojan 1:6cd6d2760856 26
ojan 0:bc6f14fc60c7 27 /********** private variables **********/
ojan 10:8ee11e412ad7 28 DigitalOut myled(LED1); // デバッグ用LEDのためのデジタル出力
ojan 10:8ee11e412ad7 29 I2C i2c(PB_9, PB_8); // I2Cポート
ojan 10:8ee11e412ad7 30 MPU6050 mpu(&i2c); // 加速度・角速度センサ
ojan 10:8ee11e412ad7 31 HMC5883L hmc(&i2c); // 地磁気センサ
ojan 10:8ee11e412ad7 32 LPS25H lps(&i2c); // 気圧センサ
ojan 10:8ee11e412ad7 33 Serial gps(PA_11, PA_12); // GPS通信用シリアルポート
ojan 10:8ee11e412ad7 34 Serial pc(SERIAL_TX, SERIAL_RX); // PC通信用シリアルポート
ojan 10:8ee11e412ad7 35 GMS6_CR6 gms(&gps, &pc); // GPS
onaka 7:0ec343d29641 36 SDFileSystem sd(PB_5, PB_4, PB_3, PB_10, "sd"); // microSD
onaka 7:0ec343d29641 37 BufferedSerial xbee(PA_9, PA_10, PC_1); // Xbee
onaka 7:0ec343d29641 38 ConfigFile cfg; //ConfigFile
ojan 10:8ee11e412ad7 39 PwmOut servoL(PB_6), servoR(PC_7); // サーボ用PWM出力
ojan 10:8ee11e412ad7 40 AnalogIn optSensor(PC_0); // 照度センサ用アナログ入力
ojan 10:8ee11e412ad7 41 AnalogIn servoVcc(PA_0); // バッテリー電圧監視用アナログ入力(サーボ用)
ojan 10:8ee11e412ad7 42 AnalogIn logicVcc(PA_1); // バッテリー電圧監視用アナログ入力(ロジック用)
ojan 10:8ee11e412ad7 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 13:df1e8a650185 75 float yaw = 0.0f; // 本体のヨー角（deg）z軸周り
ojan 13:df1e8a650185 76 float pitch = 0.0f; // 本体のピッチ角（deg）y軸周り
ojan 13:df1e8a650185 77 float roll = 0.0f; // 本体のロール角（deg）x軸周り
ojan 13:df1e8a650185 78
ojan 13:df1e8a650185 79 float vrt_acc = 0.0f; // 鉛直方向の加速度成分（落下検知に使用）
ojan 3:5358a691a100 80
onaka 7:0ec343d29641 81 /** config.txt **
onaka 7:0ec343d29641 82 * #から始めるのはコメント行
onaka 7:0ec343d29641 83 * #イコールの前後に空白を入れない
onaka 7:0ec343d29641 84 * target_x=111.222
onaka 7:0ec343d29641 85 * target_y=33.444
onaka 7:0ec343d29641 86 */
onaka 7:0ec343d29641 87 float target_x, target_y;
onaka 7:0ec343d29641 88
ojan 3:5358a691a100 89 /* ----- Kalman Filter ----- */
ojan 11:083c8c9a5b84 90 // 地磁気ベクトル用
ojan 13:df1e8a650185 91 Vector pri_x1(7);
ojan 13:df1e8a650185 92 Matrix pri_P1(7, 7);
ojan 13:df1e8a650185 93 Vector post_x1(7);
ojan 13:df1e8a650185 94 Matrix post_P1(7, 7);
ojan 13:df1e8a650185 95 Matrix F1(7, 7), H1(3, 7);
ojan 13:df1e8a650185 96 Matrix R1(7, 7), Q1(3, 3);
ojan 13:df1e8a650185 97 Matrix I1(7, 7);
ojan 13:df1e8a650185 98 Matrix K1(7, 3);
ojan 11:083c8c9a5b84 99 Matrix S1(3, 3), S_inv1(3, 3);
ojan 11:083c8c9a5b84 100
ojan 11:083c8c9a5b84 101 // 重力ベクトル用
ojan 11:083c8c9a5b84 102 // ジャイロのバイアスも同時に推定する
ojan 13:df1e8a650185 103 Vector pri_x2(5);
ojan 13:df1e8a650185 104 Matrix pri_P2(5, 5);
ojan 13:df1e8a650185 105 Vector post_x2(5);
ojan 13:df1e8a650185 106 Matrix post_P2(5, 5);
ojan 13:df1e8a650185 107 Matrix F2(5, 5), H2(3, 5);
ojan 13:df1e8a650185 108 Matrix R2(5, 5), Q2(3, 3);
ojan 13:df1e8a650185 109 Matrix I2(5, 5);
ojan 13:df1e8a650185 110 Matrix K2(5, 3);
ojan 13:df1e8a650185 111 Matrix S2(3, 3), S_inv2(3, 3);
ojan 3:5358a691a100 112 /* ----- ------------- ----- */
ojan 3:5358a691a100 113
ojan 3:5358a691a100 114 Timer timer;
ojan 3:5358a691a100 115
onaka 6:2b68f85a984a 116 char data[512] = {};
ojan 1:6cd6d2760856 117
ojan 0:bc6f14fc60c7 118 /********** private functions **********/
ojan 10:8ee11e412ad7 119 void LoadConfig(); // config読み取り
ojan 10:8ee11e412ad7 120 int find_last(); // SDカード初期化用関数
ojan 9:6d4578dcc1ed 121 void KalmanInit(); // カルマンフィルタ初期化
ojan 9:6d4578dcc1ed 122 void KalmanUpdate(); // カルマンフィルタ更新
ojan 9:6d4578dcc1ed 123 float distance(Vector p1, Vector p2); // 緯度・経度の差から2点間の距離を算出(m)
ojan 9:6d4578dcc1ed 124 void INT_func(); // 割り込み用関数
ojan 3:5358a691a100 125 void toString(Matrix& m);
ojan 3:5358a691a100 126 void toString(Vector& v);
ojan 1:6cd6d2760856 127
ojan 0:bc6f14fc60c7 128 /********** main function **********/
ojan 0:bc6f14fc60c7 129
ojan 0:bc6f14fc60c7 130 int main() {
ojan 0:bc6f14fc60c7 131
ojan 0:bc6f14fc60c7 132 i2c.frequency(400000); // I2Cの通信速度を400kHzに設定
ojan 0:bc6f14fc60c7 133
ojan 0:bc6f14fc60c7 134 if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!"); // mpu6050初期化
ojan 0:bc6f14fc60c7 135 if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!"); // hmc5883l初期化
ojan 11:083c8c9a5b84 136 /*
onaka 7:0ec343d29641 137 //Config読み取り
onaka 7:0ec343d29641 138 LoadConfig();
ojan 3:5358a691a100 139
onaka 7:0ec343d29641 140 //SDカード初期化
onaka 7:0ec343d29641 141 FILE *fp;
onaka 7:0ec343d29641 142 char filename[15];
onaka 7:0ec343d29641 143 int n = find_last();
onaka 7:0ec343d29641 144 if(n < 0){
onaka 7:0ec343d29641 145 pc.printf("Could not read a SD Card.\n");
onaka 7:0ec343d29641 146 return 0;
onaka 7:0ec343d29641 147 }
onaka 7:0ec343d29641 148 sprintf(filename, "/sd/log%03d.csv", n+1);
onaka 7:0ec343d29641 149 fp = fopen(filename, "w");
onaka 7:0ec343d29641 150 fprintf(fp, "log data\r\n");
onaka 7:0ec343d29641 151 xbee.printf("log data\r\n");
ojan 11:083c8c9a5b84 152 */
ojan 10:8ee11e412ad7 153 servoL.period(0.020f); // サーボの信号の周期は20ms
ojan 8:602865d8fca3 154 servoR.period(0.020f);
ojan 8:602865d8fca3 155
onaka 7:0ec343d29641 156 //カルマンフィルタ初期化
ojan 3:5358a691a100 157 KalmanInit();
ojan 3:5358a691a100 158
ojan 3:5358a691a100 159 INT_timer.attach(&INT_func, dt); // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
ojan 1:6cd6d2760856 160
ojan 1:6cd6d2760856 161 //重力ベクトルの初期化
ojan 4:45dc5590abc0 162 raw_g.SetComp(1, 0.0f);
ojan 4:45dc5590abc0 163 raw_g.SetComp(2, 0.0f);
ojan 4:45dc5590abc0 164 raw_g.SetComp(3, 1.0f);
ojan 0:bc6f14fc60c7 165
ojan 9:6d4578dcc1ed 166 // 機体に固定されたベクトルの初期化
ojan 13:df1e8a650185 167 b_f.SetComp(1, 0.0f);
ojan 9:6d4578dcc1ed 168 b_f.SetComp(2, 0.0f);
ojan 13:df1e8a650185 169 b_f.SetComp(3, -1.0f);
ojan 13:df1e8a650185 170 b_u.SetComp(1, 1.0f);
ojan 9:6d4578dcc1ed 171 b_u.SetComp(2, 0.0f);
ojan 13:df1e8a650185 172 b_u.SetComp(3, 0.0f);
ojan 9:6d4578dcc1ed 173 b_l = Cross(b_u, b_f);
ojan 9:6d4578dcc1ed 174
ojan 2:d2b60a1d0cd9 175 /* ---------- ↓↓↓ ここからメインループ ↓↓↓ ---------- */
ojan 0:bc6f14fc60c7 176 while(1) {
ojan 4:45dc5590abc0 177 timer.stop();
ojan 4:45dc5590abc0 178 timer.reset();
ojan 4:45dc5590abc0 179 timer.start();
ojan 3:5358a691a100 180 // 0.1秒おきにセンサーの出力をpcへ出力
ojan 4:45dc5590abc0 181 myled = 1; // LED is ON
ojan 8:602865d8fca3 182
ojan 0:bc6f14fc60c7 183
ojan 9:6d4578dcc1ed 184 // データ処理
ojan 9:6d4578dcc1ed 185 {
ojan 13:df1e8a650185 186 INT_flag = FALSE; // 割り込みによる変数書き換えを阻止
ojan 9:6d4578dcc1ed 187 gms.read(); // GPSデータ取得
ojan 9:6d4578dcc1ed 188 UTC_t = (int)gms.time;
ojan 9:6d4578dcc1ed 189
ojan 9:6d4578dcc1ed 190 // 参照座標系の基底を求める
ojan 9:6d4578dcc1ed 191 r_u = g;
ojan 9:6d4578dcc1ed 192 r_f = geomag.GetPerpCompTo(g).Normalize();
ojan 9:6d4578dcc1ed 193 r_l = Cross(r_u, r_f);
ojan 9:6d4578dcc1ed 194
ojan 11:083c8c9a5b84 195
ojan 9:6d4578dcc1ed 196 // 回転行列を経由してオイラー角を求める
ojan 9:6d4578dcc1ed 197 // オイラー角はヨー・ピッチ・ロールの順に回転したものとする
ojan 9:6d4578dcc1ed 198 // 各オイラー角を求めるのに回転行列の全成分は要らないので、一部だけ計算する。
ojan 9:6d4578dcc1ed 199
ojan 9:6d4578dcc1ed 200 float R_11 = r_f * b_f; // 回転行列の(1,1)成分
ojan 9:6d4578dcc1ed 201 float R_12 = r_f * b_l; // 回転行列の(1,2)成分
ojan 9:6d4578dcc1ed 202 float R_13 = r_f * b_u; // 回転行列の(1,3)成分
ojan 9:6d4578dcc1ed 203 float R_23 = r_l * b_u; // 回転行列の(2,3)成分
ojan 9:6d4578dcc1ed 204 float R_33 = r_u * b_u; // 回転行列の(3,3)成分
ojan 9:6d4578dcc1ed 205
ojan 13:df1e8a650185 206 yaw = atan2(-R_12, R_11) * RAD_TO_DEG - MAG_DECLINATION;
ojan 11:083c8c9a5b84 207 roll = atan2(-R_23, R_33) * RAD_TO_DEG;
ojan 11:083c8c9a5b84 208 pitch = asin(R_13) * RAD_TO_DEG;
ojan 11:083c8c9a5b84 209
ojan 13:df1e8a650185 210 pc.printf("%.3f, %.3f, %.3f\r\n",
ojan 13:df1e8a650185 211 //geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3));
ojan 13:df1e8a650185 212 //yaw, pitch, roll);
ojan 13:df1e8a650185 213 post_x2.GetComp(4),
ojan 13:df1e8a650185 214 post_x2.GetComp(5),
ojan 13:df1e8a650185 215 post_x1.GetComp(7));
ojan 9:6d4578dcc1ed 216
ojan 9:6d4578dcc1ed 217 if(UTC_t - pre_UTC_t >= 1) { // GPSデータが更新されていたら
ojan 9:6d4578dcc1ed 218 p.SetComp(1, gms.longitude * DEG_TO_RAD);
ojan 9:6d4578dcc1ed 219 p.SetComp(2, gms.latitude * DEG_TO_RAD);
ojan 9:6d4578dcc1ed 220 } else { // 更新されていなかったら
ojan 9:6d4578dcc1ed 221
ojan 9:6d4578dcc1ed 222 }
ojan 9:6d4578dcc1ed 223
ojan 9:6d4578dcc1ed 224 pre_p = p;
ojan 9:6d4578dcc1ed 225 pre_UTC_t = UTC_t;
ojan 13:df1e8a650185 226
ojan 13:df1e8a650185 227 float sv = (float)servoVcc.read_u16() * ADC_LSB_TO_V * 2.0f;
ojan 13:df1e8a650185 228 float lv = (float)logicVcc.read_u16() * ADC_LSB_TO_V * 2.0f;
ojan 13:df1e8a650185 229
ojan 13:df1e8a650185 230 // データをmicroSDに保存し、XBeeでPCへ送信する
ojan 13:df1e8a650185 231 sprintf(data, "%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%d\r\n",
ojan 13:df1e8a650185 232 g.GetComp(1), g.GetComp(2), g.GetComp(3),
ojan 13:df1e8a650185 233 geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3),
ojan 13:df1e8a650185 234 press, gms.longitude, gms.latitude,
ojan 13:df1e8a650185 235 sv, lv, optSensor.read_u16());
ojan 13:df1e8a650185 236 //fprintf(fp, data);
ojan 13:df1e8a650185 237 //fflush(fp);
ojan 13:df1e8a650185 238 //xbee.printf(data);
ojan 13:df1e8a650185 239
ojan 13:df1e8a650185 240 INT_flag = TRUE; // 割り込み許可
ojan 9:6d4578dcc1ed 241 }
ojan 9:6d4578dcc1ed 242
ojan 0:bc6f14fc60c7 243
ojan 5:182f6356bce1 244 // 制御ルーチン
ojan 9:6d4578dcc1ed 245 {
ojan 13:df1e8a650185 246
ojan 13:df1e8a650185 247 if(fabs(roll) > 40.0f) {
ojan 13:df1e8a650185 248 // スパイラル回避行動
ojan 13:df1e8a650185 249 if(roll > 0) {
ojan 13:df1e8a650185 250 pull_L = PullMax;
ojan 13:df1e8a650185 251 pull_R = 0;
ojan 13:df1e8a650185 252 } else {
ojan 13:df1e8a650185 253 pull_L = 0;
ojan 13:df1e8a650185 254 pull_R = PullMax;
ojan 13:df1e8a650185 255 }
ojan 13:df1e8a650185 256 } else {
ojan 13:df1e8a650185 257
ojan 13:df1e8a650185 258 }
ojan 13:df1e8a650185 259
ojan 13:df1e8a650185 260 //pull_L = (pull_L+5)%PullMax;
ojan 13:df1e8a650185 261 //pull_R = (pull_R+5)%PullMax;
ojan 11:083c8c9a5b84 262 //pull_L = 0;
ojan 11:083c8c9a5b84 263 //pull_R = 30;
ojan 13:df1e8a650185 264
ojan 13:df1e8a650185 265
ojan 13:df1e8a650185 266
ojan 13:df1e8a650185 267 // 指定された引っ張り量だけ紐を引っ張る
ojan 13:df1e8a650185 268 if(pull_L < 0) pull_L = 0;
ojan 13:df1e8a650185 269 else if(pull_L > PullMax) pull_L = PullMax;
ojan 13:df1e8a650185 270 if(pull_R < 0) pull_R = 0;
ojan 13:df1e8a650185 271 else if(pull_R > PullMax) pull_R = PullMax;
ojan 13:df1e8a650185 272
ojan 9:6d4578dcc1ed 273 servoL.pulsewidth((ServoMax - ServoMin) * pull_L / PullMax + ServoMin);
ojan 9:6d4578dcc1ed 274 servoR.pulsewidth((ServoMax - ServoMin) * pull_R / PullMax + ServoMin);
ojan 8:602865d8fca3 275
ojan 9:6d4578dcc1ed 276 }
ojan 9:6d4578dcc1ed 277
ojan 9:6d4578dcc1ed 278 myled = 0; // LED is OFF
ojan 11:083c8c9a5b84 279 /*pc.printf("%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n",
ojan 11:083c8c9a5b84 280 post_x.GetComp(4), post_x.GetComp(5), post_x.GetComp(6),
ojan 11:083c8c9a5b84 281 yaw, pitch, roll,
ojan 11:083c8c9a5b84 282 geomag.GetNorm());*/
ojan 5:182f6356bce1 283
ojan 9:6d4578dcc1ed 284 // ループはきっかり0.2秒ごと
ojan 9:6d4578dcc1ed 285 while(timer.read_ms() < 200);
ojan 5:182f6356bce1 286
ojan 4:45dc5590abc0 287
ojan 0:bc6f14fc60c7 288 }
ojan 2:d2b60a1d0cd9 289
ojan 2:d2b60a1d0cd9 290 /* ---------- ↑↑↑ ここまでメインループ ↑↑↑ ---------- */
onaka 7:0ec343d29641 291 //fclose(fp);
onaka 7:0ec343d29641 292 }
onaka 7:0ec343d29641 293
onaka 7:0ec343d29641 294 void LoadConfig(){
onaka 7:0ec343d29641 295 char value[20];
onaka 7:0ec343d29641 296 //Read a configuration file from a mbed.
onaka 7:0ec343d29641 297 if (!cfg.read("/sd/config.txt")){
onaka 7:0ec343d29641 298 pc.printf("Config file does not exist\n");
onaka 7:0ec343d29641 299 }else{
onaka 7:0ec343d29641 300 //Get values
onaka 7:0ec343d29641 301 if (cfg.getValue("target_x", &value[0], sizeof(value))) target_x = atof(value);
onaka 7:0ec343d29641 302 else{
onaka 7:0ec343d29641 303 pc.printf("Failed to get value for target_x\n");
onaka 7:0ec343d29641 304 }
onaka 7:0ec343d29641 305 if (cfg.getValue("target_y", &value[0], sizeof(value))) target_y = atof(value);
onaka 7:0ec343d29641 306 else{
onaka 7:0ec343d29641 307 pc.printf("Failed to get value for target_y\n");
onaka 7:0ec343d29641 308 }
onaka 7:0ec343d29641 309 }
onaka 7:0ec343d29641 310 }
onaka 7:0ec343d29641 311
onaka 7:0ec343d29641 312 int find_last() {
onaka 7:0ec343d29641 313 int i, n = 0;
onaka 7:0ec343d29641 314 char c;
onaka 7:0ec343d29641 315 DIR *dp;
onaka 7:0ec343d29641 316 struct dirent *dirst;
onaka 7:0ec343d29641 317 dp = opendir("/sd/");
onaka 7:0ec343d29641 318 if (!dp){
onaka 7:0ec343d29641 319 pc.printf("Could not open directry.\n");
onaka 7:0ec343d29641 320 return -1;
onaka 7:0ec343d29641 321 }
onaka 7:0ec343d29641 322 while((dirst = readdir(dp)) != NULL) {
onaka 7:0ec343d29641 323 if(sscanf(dirst->d_name, "log%03d.csv%c", &i, &c) == 1 && i>n) {
onaka 7:0ec343d29641 324 n = i;
onaka 7:0ec343d29641 325 }
onaka 7:0ec343d29641 326 }
onaka 7:0ec343d29641 327 closedir(dp);
onaka 7:0ec343d29641 328 return n;
ojan 0:bc6f14fc60c7 329 }
ojan 0:bc6f14fc60c7 330
ojan 3:5358a691a100 331 void KalmanInit() {
ojan 11:083c8c9a5b84 332 // 重力
ojan 11:083c8c9a5b84 333 {
ojan 11:083c8c9a5b84 334 // 誤差共分散行列の値を決める（対角成分のみ）
ojan 13:df1e8a650185 335 float alpha_R2 = 0.002f;
ojan 12:0d978eb4d639 336 float alpha_Q2 = 0.5f;
ojan 11:083c8c9a5b84 337 R2 *= alpha_R2;
ojan 11:083c8c9a5b84 338 Q2 *= alpha_Q2;
ojan 11:083c8c9a5b84 339
ojan 13:df1e8a650185 340 // 観測方程式のヤコビアンの値を設定（時間変化無し）
ojan 13:df1e8a650185 341 float h2[15] = {
ojan 13:df1e8a650185 342 1.0f, 0.0f, 0.0f, 0.0f, 0.0f,
ojan 13:df1e8a650185 343 0.0f, 1.0f, 0.0f, 0.0f, 0.0f,
ojan 13:df1e8a650185 344 0.0f, 0.0f, 1.0f, 0.0f, 0.0f
ojan 11:083c8c9a5b84 345 };
ojan 11:083c8c9a5b84 346
ojan 13:df1e8a650185 347 H2.SetComps(h2);
ojan 11:083c8c9a5b84 348 }
ojan 3:5358a691a100 349
ojan 11:083c8c9a5b84 350 // 地磁気
ojan 11:083c8c9a5b84 351 {
ojan 11:083c8c9a5b84 352 // 誤差共分散行列の値を決める（対角成分のみ）
ojan 13:df1e8a650185 353 float alpha_R1 = 0.002f;
ojan 13:df1e8a650185 354 float alpha_Q1 = 0.5f;
ojan 11:083c8c9a5b84 355 R1 *= alpha_R1;
ojan 11:083c8c9a5b84 356 Q1 *= alpha_Q1;
ojan 11:083c8c9a5b84 357
ojan 13:df1e8a650185 358 // 観測方程式のヤコビアンの値を設定（時間変化無し）
ojan 13:df1e8a650185 359 float h1[21] = {
ojan 13:df1e8a650185 360 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f,
ojan 13:df1e8a650185 361 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
ojan 13:df1e8a650185 362 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f
ojan 11:083c8c9a5b84 363 };
ojan 11:083c8c9a5b84 364
ojan 13:df1e8a650185 365 H1.SetComps(h1);
ojan 11:083c8c9a5b84 366 }
ojan 3:5358a691a100 367 }
ojan 3:5358a691a100 368
ojan 3:5358a691a100 369 void KalmanUpdate() {
ojan 13:df1e8a650185 370 // 重力
ojan 13:df1e8a650185 371
ojan 11:083c8c9a5b84 372 {
ojan 13:df1e8a650185 373 // ヤコビアンの更新
ojan 13:df1e8a650185 374 float f2[25] = {
ojan 13:df1e8a650185 375 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 13:df1e8a650185 376 -(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 13:df1e8a650185 377 (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 13:df1e8a650185 378 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,
ojan 13:df1e8a650185 379 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
ojan 13:df1e8a650185 380 };
ojan 13:df1e8a650185 381
ojan 13:df1e8a650185 382 F2.SetComps(f2);
ojan 11:083c8c9a5b84 383
ojan 11:083c8c9a5b84 384 // 事前推定値の更新
ojan 13:df1e8a650185 385 //pri_x2 = F2 * post_x2;
ojan 13:df1e8a650185 386
ojan 13:df1e8a650185 387 float pri_x2_vals[5] = {
ojan 13:df1e8a650185 388 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 13:df1e8a650185 389 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 13:df1e8a650185 390 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 13:df1e8a650185 391 post_x2.GetComp(4),
ojan 13:df1e8a650185 392 post_x2.GetComp(5)
ojan 13:df1e8a650185 393 };
ojan 13:df1e8a650185 394
ojan 13:df1e8a650185 395 pri_x2.SetComps(pri_x2_vals);
ojan 13:df1e8a650185 396
ojan 11:083c8c9a5b84 397 // 事前誤差分散行列の更新
ojan 11:083c8c9a5b84 398 pri_P2 = F2 * post_P2 * F2.Transpose() + R2;
ojan 11:083c8c9a5b84 399
ojan 11:083c8c9a5b84 400 // カルマンゲインの計算
ojan 11:083c8c9a5b84 401 S2 = Q2 + H2 * pri_P2 * H2.Transpose();
ojan 13:df1e8a650185 402 static float det;
ojan 11:083c8c9a5b84 403 if((det = S2.Inverse(S_inv2)) >= 0.0f) {
ojan 11:083c8c9a5b84 404 pc.printf("E:%.3f\r\n", det);
ojan 11:083c8c9a5b84 405 return; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了
ojan 11:083c8c9a5b84 406 }
ojan 11:083c8c9a5b84 407 K2 = pri_P2 * H2.Transpose() * S_inv2;
ojan 11:083c8c9a5b84 408
ojan 11:083c8c9a5b84 409 // 事後推定値の更新
ojan 13:df1e8a650185 410 post_x2 = pri_x2 + K2 * (raw_acc - H2 * pri_x2);
ojan 11:083c8c9a5b84 411 // 事後誤差分散行列の更新
ojan 11:083c8c9a5b84 412 post_P2 = (I2 - K2 * H2) * pri_P2;
ojan 11:083c8c9a5b84 413 }
ojan 3:5358a691a100 414
ojan 13:df1e8a650185 415
ojan 11:083c8c9a5b84 416 // 地磁気
ojan 11:083c8c9a5b84 417 {
ojan 11:083c8c9a5b84 418 // ヤコビアンの更新
ojan 13:df1e8a650185 419 float f1[49] = {
ojan 13:df1e8a650185 420 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 13:df1e8a650185 421 -(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 13:df1e8a650185 422 (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 13:df1e8a650185 423 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f,
ojan 13:df1e8a650185 424 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
ojan 13:df1e8a650185 425 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,
ojan 13:df1e8a650185 426 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
ojan 11:083c8c9a5b84 427 };
ojan 11:083c8c9a5b84 428
ojan 13:df1e8a650185 429 F1.SetComps(f1);
ojan 11:083c8c9a5b84 430
ojan 11:083c8c9a5b84 431 // 事前推定値の更新
ojan 13:df1e8a650185 432 //pri_x1 = F1 * post_x1;
ojan 13:df1e8a650185 433 float pri_x1_vals[7] = {
ojan 13:df1e8a650185 434 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 13:df1e8a650185 435 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 13:df1e8a650185 436 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 13:df1e8a650185 437 post_x1.GetComp(4),
ojan 13:df1e8a650185 438 post_x1.GetComp(5),
ojan 13:df1e8a650185 439 post_x1.GetComp(6),
ojan 13:df1e8a650185 440 post_x1.GetComp(7)
ojan 13:df1e8a650185 441 };
ojan 13:df1e8a650185 442
ojan 13:df1e8a650185 443 pri_x1.SetComps(pri_x1_vals);
ojan 13:df1e8a650185 444
ojan 11:083c8c9a5b84 445 // 事前誤差分散行列の更新
ojan 11:083c8c9a5b84 446 pri_P1 = F1 * post_P1 * F1.Transpose() + R1;
ojan 11:083c8c9a5b84 447
ojan 11:083c8c9a5b84 448 // カルマンゲインの計算
ojan 11:083c8c9a5b84 449 S1 = Q1 + H1 * pri_P1 * H1.Transpose();
ojan 13:df1e8a650185 450 static float det;
ojan 11:083c8c9a5b84 451 if((det = S1.Inverse(S_inv1)) >= 0.0f) {
ojan 11:083c8c9a5b84 452 pc.printf("E:%.3f\r\n", det);
ojan 11:083c8c9a5b84 453 return; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了
ojan 11:083c8c9a5b84 454 }
ojan 11:083c8c9a5b84 455 K1 = pri_P1 * H1.Transpose() * S_inv1;
ojan 11:083c8c9a5b84 456
ojan 11:083c8c9a5b84 457 // 事後推定値の更新
ojan 11:083c8c9a5b84 458 post_x1 = pri_x1 + K1 * (raw_geomag - H1 * pri_x1);
ojan 13:df1e8a650185 459 // 地磁気ベクトルの大きさに拘束条件を与える
ojan 13:df1e8a650185 460 /*{
ojan 13:df1e8a650185 461 Vector gm(3);
ojan 13:df1e8a650185 462 gm.SetComp(1, post_x1.GetComp(1));
ojan 13:df1e8a650185 463 gm.SetComp(2, post_x1.GetComp(2));
ojan 13:df1e8a650185 464 gm.SetComp(3, post_x1.GetComp(3));
ojan 13:df1e8a650185 465
ojan 13:df1e8a650185 466 gm = MAG_MAGNITUDE * gm.Normalize();
ojan 13:df1e8a650185 467
ojan 13:df1e8a650185 468 post_x1.SetComp(1, gm.GetComp(1));
ojan 13:df1e8a650185 469 post_x1.SetComp(2, gm.GetComp(2));
ojan 13:df1e8a650185 470 post_x1.SetComp(3, gm.GetComp(3));
ojan 13:df1e8a650185 471 }*/
ojan 11:083c8c9a5b84 472 // 事後誤差分散行列の更新
ojan 11:083c8c9a5b84 473 post_P1 = (I1 - K1 * H1) * pri_P1;
ojan 3:5358a691a100 474 }
ojan 3:5358a691a100 475 }
ojan 3:5358a691a100 476
ojan 9:6d4578dcc1ed 477 float distance(Vector p1, Vector p2) {
ojan 9:6d4578dcc1ed 478 if(p1.GetDim() != p2.GetDim()) return 0.0f;
ojan 9:6d4578dcc1ed 479
ojan 13:df1e8a650185 480 static float mu_y = (p1.GetComp(2) + p2.GetComp(2)) * 0.5f;
ojan 13:df1e8a650185 481 static float s_mu_y = sin(mu_y);
ojan 13:df1e8a650185 482 static float w = sqrt(1 - GPS_SQ_E * s_mu_y * s_mu_y);
ojan 13:df1e8a650185 483 static float m = GPS_A * (1 - GPS_SQ_E) / (w * w * w);
ojan 13:df1e8a650185 484 static float n = GPS_A / w;
ojan 13:df1e8a650185 485 static float d1 = m * (p1.GetComp(2) - p2.GetComp(2));
ojan 13:df1e8a650185 486 static float d2 = n * cos(mu_y) * (p1.GetComp(1) - p2.GetComp(1));
ojan 9:6d4578dcc1ed 487
ojan 9:6d4578dcc1ed 488 return sqrt(d1 * d1 + d2 * d2);
ojan 9:6d4578dcc1ed 489 }
ojan 9:6d4578dcc1ed 490
ojan 9:6d4578dcc1ed 491 /* -------------------- 割り込み関数 -------------------- */
ojan 9:6d4578dcc1ed 492
ojan 0:bc6f14fc60c7 493 void INT_func() {
ojan 4:45dc5590abc0 494 // センサーの値を更新
ojan 4:45dc5590abc0 495 mpu.read();
ojan 4:45dc5590abc0 496 hmc.read();
ojan 4:45dc5590abc0 497
ojan 4:45dc5590abc0 498 for(int i=0; i<3; i++) {
ojan 4:45dc5590abc0 499 raw_acc.SetComp(i+1, (float)mpu.data.value.acc[i] * ACC_LSB_TO_G);
ojan 4:45dc5590abc0 500 raw_gyro.SetComp(i+1, (float)mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD);
ojan 4:45dc5590abc0 501 raw_geomag.SetComp(i+1, (float)hmc.data.value[i] * MAG_LSB_TO_GAUSS);
ojan 4:45dc5590abc0 502 }
ojan 0:bc6f14fc60c7 503
ojan 4:45dc5590abc0 504 Vector delta_g = Cross(raw_gyro, raw_g); // Δg = ω × g
ojan 4:45dc5590abc0 505 raw_g = 0.9f * (raw_g - delta_g * dt) + 0.1f * raw_acc.Normalize(); // 相補フィルタ
ojan 4:45dc5590abc0 506 raw_g = raw_g.Normalize();
ojan 4:45dc5590abc0 507
ojan 4:45dc5590abc0 508 KalmanUpdate();
ojan 0:bc6f14fc60c7 509
ojan 4:45dc5590abc0 510 // LPS25Hによる気圧の取得は10Hz
ojan 4:45dc5590abc0 511 lps_cnt = (lps_cnt+1)%10;
ojan 4:45dc5590abc0 512 if(lps_cnt == 0) {
ojan 4:45dc5590abc0 513 raw_press = (float)lps.pressure() * PRES_LSB_TO_HPA;
ojan 4:45dc5590abc0 514 }
ojan 4:45dc5590abc0 515 //pc.printf("%d(us)\r\n", timer.read_us());
ojan 4:45dc5590abc0 516
ojan 4:45dc5590abc0 517 if(INT_flag != FALSE) {
ojan 4:45dc5590abc0 518 g = raw_g;
ojan 4:45dc5590abc0 519 for(int i=0; i<3; i++) {
ojan 11:083c8c9a5b84 520 geomag.SetComp(i+1, post_x1.GetComp(i+1));
ojan 4:45dc5590abc0 521 }
ojan 4:45dc5590abc0 522 acc = raw_acc;
ojan 4:45dc5590abc0 523 gyro = raw_gyro;
ojan 4:45dc5590abc0 524 press = raw_press;
ojan 1:6cd6d2760856 525
ojan 13:df1e8a650185 526 vrt_acc = raw_acc * raw_g;
ojan 13:df1e8a650185 527
ojan 0:bc6f14fc60c7 528 }
ojan 3:5358a691a100 529 }
ojan 3:5358a691a100 530
ojan 9:6d4578dcc1ed 531 /* -------------------- デバッグ用関数 -------------------- */
ojan 9:6d4578dcc1ed 532
ojan 3:5358a691a100 533 void toString(Matrix& m) {
ojan 3:5358a691a100 534
ojan 3:5358a691a100 535 for(int i=0; i<m.GetRow(); i++) {
ojan 3:5358a691a100 536 for(int j=0; j<m.GetCol(); j++) {
ojan 3:5358a691a100 537 pc.printf("%.6f\t", m.GetComp(i+1, j+1));
ojan 3:5358a691a100 538 }
ojan 3:5358a691a100 539 pc.printf("\r\n");
ojan 3:5358a691a100 540 }
ojan 3:5358a691a100 541
ojan 3:5358a691a100 542 }
ojan 3:5358a691a100 543
ojan 3:5358a691a100 544 void toString(Vector& v) {
ojan 3:5358a691a100 545
ojan 3:5358a691a100 546 for(int i=0; i<v.GetDim(); i++) {
ojan 3:5358a691a100 547 pc.printf("%.6f\t", v.GetComp(i+1));
ojan 3:5358a691a100 548 }
ojan 3:5358a691a100 549 pc.printf("\r\n");
ojan 3:5358a691a100 550
ojan 0:bc6f14fc60c7 551 }