hiroya taura / Mbed 2 deprecated LAURUS_program

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

Dependencies:   ConfigFile SDFileSystem mbed

Fork of LAURUS_program by LAURUS

main.cpp@34:4bda9af9a0cd, 2015-08-06 (annotated)

Committer:
ojan
Date:
Thu Aug 06 15:45:48 2015 +0000
Revision:
34:4bda9af9a0cd
Parent:
33:56163d4e2c53
Child:
36:94dc027e05cd
LAURUS_Program_v2.9.0; ; + add RULE3_1; + add "volatile" to some variables like "flag"

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