hiroya taura / Mbed 2 deprecated LAURUS_program

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

Dependencies:   ConfigFile SDFileSystem mbed

Fork of LAURUS_program by LAURUS

main.cpp@14:f85cb5340cb8, 2015-06-19 (annotated)

Committer:
ojan
Date:
Fri Jun 19 15:56:52 2015 +0000
Revision:
14:f85cb5340cb8
Parent:
13:df1e8a650185
Child:
15:d14d385d37e2
LAURUS_controller_scripts_v1.0

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