hiroya taura / Mbed 2 deprecated LAURUS_program

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

Dependencies:   ConfigFile SDFileSystem mbed

Fork of LAURUS_program by LAURUS

main.cpp@19:ad8ff2de68f5, 2015-06-22 (annotated)

Committer:
ojan
Date:
Mon Jun 22 15:23:37 2015 +0000
Revision:
19:ad8ff2de68f5
Parent:
18:9dd72e417c60
Parent:
17:03b45055ca05
Child:
20:00afba164688
LAURUS_Program_v.2.1; ; + some refactoring; + GPS bug fixed (by Onaka

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