MPUとHMCでうごくかもver

Dependencies:   ConfigFile SDFileSystem mbed

Fork of LAURUS_program by hiroya taura

Committer:
ojan
Date:
Sun Jun 28 05:48:46 2015 +0000
Revision:
26:6e09df57ee91
Parent:
25:4c72d7420d8a
Child:
27:a26ff85bba23
LAURUS_Program_v2.5.1

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