MPUとHMCでうごくかもver
Dependencies: ConfigFile SDFileSystem mbed
Fork of LAURUS_program by
main.cpp
- Committer:
- ojan
- Date:
- 2015-06-22
- Revision:
- 20:00afba164688
- Parent:
- 19:ad8ff2de68f5
- Child:
- 21:d417708e84a8
File content as of revision 20:00afba164688:
#include "mbed.h" #include "MPU6050.h" #include "HMC5883L.h" #include "LPS25H.h" #include "GMS6_CR6.h" #include "ErrorLogger.h" #include "Vector.h" #include "Matrix.h" #include "Vector_Matrix_operator.h" #include "myConstants.h" #include "SDFileSystem.h" #include "BufferedSerial.h" #include "ConfigFile.h" /****************************** private define ******************************/ #define TRUE 1 #define FALSE 0 #define RULE1 //#define RULE2 //#define RULE3 const float dt = 0.01f; // 割り込み周期(s) const float ServoMax = 0.0023f; // サーボの最大パルス長(s) const float ServoMin = 0.0006f; // サーボの最小パルス長(s) const float PullMax = 25.0f; // 引っ張れる紐の最大量(mm) const float BorderSpiral = 40.0f; // スパイラル検知角度 const short BorderOpt = 30000; // 光センサーの閾値 const float BorderGravity = 0.3f; // 無重力状態の閾値 const int BorderParafoil = 0; // 物理スイッチのOFF出力 const int MaxCount = 3; // 投下シグナルを何回連続で検知したら投下と判断するか(×0.2[s]) const int WaitTime = 1; // 投下後、安定するまで何秒滑空するか const float Alpha = 30.0f; // 目標方向と自分の進行方向との差の閾値(deg)(制御則1&2&3の定数 const float Beta = 60.0f; // 目標方向と自分の進行方向との間に取るべき角度差(deg)(制御則3の定数 const float BorderDistance = 10.0f; // 落下制御に入るための目標値との距離の閾値(m)(制御則2の定数 /****************************** private macro ******************************/ /****************************** private typedef ******************************/ /****************************** private variables ******************************/ DigitalOut myled(LED1); // デバッグ用LEDのためのデジタル出力 I2C i2c(PB_9, PB_8); // I2Cポート MPU6050 mpu(&i2c); // 加速度・角速度センサ HMC5883L hmc(&i2c); // 地磁気センサ LPS25H lps(&i2c); // 気圧センサ Serial gps(PA_11, PA_12); // GPS通信用シリアルポート Serial pc(SERIAL_TX, SERIAL_RX); // PC通信用シリアルポート GMS6_CR6 gms(&gps, &pc); // GPS SDFileSystem sd(PB_5, PB_4, PB_3, PB_10, "sd"); // microSD FILE * fp; // ログファイルのポインタ BufferedSerial xbee(PA_9, PA_10, PC_1); // Xbee //Serial xbee(PA_9, PA_10); // XBee //DigitalIn cts(PC_1); // ctsピン ConfigFile cfg; // ConfigFile PwmOut servoL(PB_6), servoR(PC_7); // サーボ用PWM出力 AnalogIn optSensor(PC_0); // 照度センサ用アナログ入力 AnalogIn servoVcc(PA_0); // バッテリー電圧監視用アナログ入力(サーボ用) AnalogIn logicVcc(PA_1); // バッテリー電圧監視用アナログ入力(ロジック用) DigitalIn paraSensor(PB_0); // パラフォイルに繋がる(予定)の物理スイッチ Ticker ticker; // 割り込みタイマー Timer timer; // 時間計測用タイマー int lps_cnt = 0; // 気圧センサ読み取りカウント uint8_t INT_flag = TRUE; // 割り込み可否フラグ /* こちらの変数群はメインループでは参照しない */ Vector raw_acc(3); // 加速度(m/s^2) 生 Vector raw_gyro(3); // 角速度(deg/s) 生 Vector raw_geomag(3); // 地磁気(?) 生 float raw_press; // 気圧(hPa) 生 /* メインループ内ではこちらを参照する */ Vector acc(3); // 加速度(m/s^2) Vector gyro(3); // 角速度(deg/s) Vector geomag(3); // 地磁気(?) float press; // 気圧(hPa) Vector raw_g(3); // 重力ベクトル 生 Vector g(3); // 重力ベクトル Vector target_p(2); // 目標情報(経度、緯度)(rad) Vector p(2); // 位置情報(経度, 緯度)(rad) Vector pre_p(2); // 過去の位置情報(経度, 緯度)(rad) int UTC_t = 0; // UTC時刻 int pre_UTC_t = 0; // 前のUTC時刻 Vector b_f(3); // 機体座標に固定された、機体前方向きのベクトル(x軸) Vector b_u(3); // 機体座標に固定された、機体上方向きのベクトル(z軸) Vector b_l(3); // 機体座標に固定された、機体左方向きのベクトル(y軸) Vector r_f(3); // 世界座標に固定された、北向きのベクトル(X軸) Vector r_u(3); // 世界座標に固定された、上向きのベクトル(Z軸) Vector r_l(3); // 世界座標に固定された、西向きのベクトル(Y軸) int pull_L = 0; // 左サーボの引っ張り量(mm:0~PullMax) int pull_R = 0; // 右サーボの引っ張り量(mm:0~PullMax) float yaw = 0.0f; // 本体のヨー角(deg)z軸周り float pitch = 0.0f; // 本体のピッチ角(deg)y軸周り float roll = 0.0f; // 本体のロール角(deg)x軸周り float vrt_acc = 0.0f; // 鉛直方向の加速度成分(落下検知に使用) int step = 0; // シーケンス制御のステップ int count = 0; // 安定滑空までの時間測定用カウンター int dir = 0; // 旋回方向(0:左 1:右) char data[512] = {}; // 送信データ用配 int loopTime = 0; // 1ループに掛かる時間(デバッグ用 /** config.txt ** * #から始めるのはコメント行 * #イコールの前後に空白を入れない * target_x=111.222 * target_y=33.444 */ float target_x, target_y; /* ---------- Kalman Filter ---------- */ // 地磁気ベクトル用 // ジャイロのz軸周りのバイアスも推定 Vector pri_x1(7); Matrix pri_P1(7, 7); Vector post_x1(7); Matrix post_P1(7, 7); Matrix F1(7, 7), H1(3, 7); Matrix R1(7, 7), Q1(3, 3); Matrix I1(7, 7); Matrix K1(7, 3); Matrix S1(3, 3), S_inv1(3, 3); // 重力ベクトル用 // ジャイロのx軸、y軸周りのバイアスも同時に推定 Vector pri_x2(5); Matrix pri_P2(5, 5); Vector post_x2(5); Matrix post_P2(5, 5); Matrix F2(5, 5), H2(3, 5); Matrix R2(5, 5), Q2(3, 3); Matrix I2(5, 5); Matrix K2(5, 3); Matrix S2(3, 3), S_inv2(3, 3); /* ---------- ------------- ---------- */ /****************************** private functions ******************************/ void SD_Init(); // SDカード初期化 void VectorsInit(); // 各種ベクトルの初期化 void KalmanInit(); // カルマンフィルタ初期化 void LoadConfig(); // config読み取り int Find_last(); // SDカード初期化用関数 void DataProcessing(); // データ処理関数 void ControlRoutine(); // 制御ルーチン関数 void KalmanUpdate(); // カルマンフィルタ更新 float Distance(Vector p1, Vector p2); // 緯度・経度の差から2点間の距離を算出(m) bool Thrown(); // 投下されたかどうかを判断する void DataUpdate(); // データ取得&更新関数 void toString(Matrix& m); // デバッグ用 void toString(Vector& v); // デバッグ用 inline float min(float a, float b) { return ((a > b) ? b : a); } /****************************** main function ******************************/ int main() { i2c.frequency(400000); // I2Cの通信速度を400kHzに設定 if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!"); // mpu6050初期化 if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!"); // hmc5883l初期化 //Config読み取り LoadConfig(); // SDカード初期化 SD_Init(); //カルマンフィルタ初期化 KalmanInit(); // 各種ベクトルの初期化 VectorsInit(); ticker.attach(&DataUpdate, dt); // 割り込み有効化(Freq = 0.01fなので、10msおきの割り込み) NVIC_SetPriority(TIM5_IRQn, 5); servoL.period(0.020f); // サーボの信号の周期は20ms servoR.period(0.020f); /* ------------------------------ ↓↓↓ ここからメインループ ↓↓↓ ------------------------------ */ while(1) { timer.stop(); timer.reset(); timer.start(); myled = 1; // LED is ON /******************************* データ処理 *******************************/ DataProcessing(); /******************************* 制御ルーチン *******************************/ ControlRoutine(); myled = 0; // LED is OFF loopTime = timer.read_ms(); // ループはきっかり0.2秒ごと while(timer.read_ms() < 200); } /* ------------------------------ ↑↑↑ ここまでメインループ ↑↑↑ ------------------------------ */ } /** データ処理関数 * */ void DataProcessing() { INT_flag = FALSE; // 割り込みによる変数書き換えを阻止 gms.read(); // GPSデータ取得 UTC_t = (int)gms.time; // 参照座標系の基底を求める r_u = g; r_f = geomag.GetPerpCompTo(g).Normalize(); r_l = Cross(r_u, r_f); // 回転行列を経由してオイラー角を求める // オイラー角はヨー・ピッチ・ロールの順に回転したものとする // 各オイラー角を求めるのに回転行列の全成分は要らないので、一部だけ計算する。 float R_11 = r_f * b_f; // 回転行列の(1,1)成分 float R_12 = r_f * b_l; // 回転行列の(1,2)成分 float R_13 = r_f * b_u; // 回転行列の(1,3)成分 float R_23 = r_l * b_u; // 回転行列の(2,3)成分 float R_33 = r_u * b_u; // 回転行列の(3,3)成分 #ifdef RULE3 yaw = atan2(-R_12, R_11) * RAD_TO_DEG + MAG_DECLINATION - Beta; #else yaw = atan2(-R_12, R_11) * RAD_TO_DEG + MAG_DECLINATION; #endif roll = atan2(-R_23, R_33) * RAD_TO_DEG; pitch = asin(R_13) * RAD_TO_DEG; if(yaw < -180.0f) yaw += 360.0f; // ヨー角を[-π, π]に補正 if(yaw > 180.0f) yaw -= 360.0f; // ヨー角を[-π, π]に補正 if(UTC_t - pre_UTC_t >= 1) { // GPSデータが更新されていたら p.SetComp(1, gms.longitude * DEG_TO_RAD); p.SetComp(2, gms.latitude * DEG_TO_RAD); } else { // 更新されていなかったら // GPSの補完処理を追加予定 } pre_p = p; pre_UTC_t = UTC_t; float sv = (float)servoVcc.read_u16() * ADC_LSB_TO_V * 2.0f; // サーボ電源電圧 float lv = (float)logicVcc.read_u16() * ADC_LSB_TO_V * 2.0f; // ロジック電源電圧 // データをmicroSDに保存し、XBeeでPCへ送信する sprintf(data, "%d, %.1f,%.1f,%.1f, %.3f,%.5f,%.5f, %.3f,%.3f,%d, %.3f,%d\r\n", UTC_t, yaw, pitch, roll, press, gms.longitude, gms.latitude, lv, vrt_acc, loopTime, Distance(target_p, p), optSensor.read_u16()); fprintf(fp, data); fflush(fp); xbee.printf(data); /* int dataLength = strlen(data); for(int i=0; i<dataLength; i++) { while(cts); xbee.putc(data[i]); } */ INT_flag = TRUE; // 割り込み許可 } /** 制御ルーチン関数 * */ void ControlRoutine() { switch(step) { // 投下&安定滑空シーケンス case 0: if(Thrown() || count != 0) { count++; // 投下直後に紐を引く場合はコメントアウトをはずす // pull_L = 15; // pull_R = 15; if(count >= WaitTime*5) { pull_L = 0; pull_R = 0; step = 1; } } break; // 制御シーケンス case 1: if(fabs(roll) > BorderSpiral) { // スパイラル回避行動 if(roll > 0) { pull_L = PullMax; pull_R = 0; } else { pull_L = 0; pull_R = PullMax; } } else { /* いずれも地球を完全球体と仮定 */ float target_lng = target_x * DEG_TO_RAD; float target_lat = target_y * DEG_TO_RAD; /* 北から西回りで目標方向の角度を出力 */ float targetY = cos( target_lat ) * sin( target_lng - p.GetComp(1) ); float targetX = cos( p.GetComp(2) ) * sin( target_lat ) - sin( p.GetComp(2) ) * cos( target_lat ) * cos( target_lng - p.GetComp(1) ); float theta = -atan2f( targetY, targetX ); float delta_theta = 0.0f; if(yaw >= 0.0f) { // ヨー角が正 if(theta >= 0.0f) { // 目標角も正ならば、 if(theta - yaw > Alpha) dir = 0; // 単純に差を取って閾値αと比べる else if(theta - yaw < -Alpha) dir = 1; } else { // 目標角が負であれば theta += 360.0f; // 360°足して正の値に変換してから delta_theta = theta - yaw; // 差を取る(yawから左回りに見たthetaとの差分) if(delta_theta < 180.0f) { // 差が180°より小さければ左旋回 if(delta_theta > Alpha) dir = 0; } else { // 180°より大きければ右旋回 if(360.0f - delta_theta > Alpha) dir = 1; } } } else { // ヨー角が負 if(theta <= 0.0f) { // 目標角も負ならば、 if(theta - yaw > Alpha) dir = 0; // 単純に差を取って閾値αと比べる else if(theta - yaw < -Alpha) dir = 1; } else { // 目標角が正であれば、 delta_theta = theta - yaw; // 差を取る(yawから左回りに見たthetaとの差分) if(delta_theta < 180.0f) { // 差が180°より小さければ左旋回 if(delta_theta > Alpha) dir = 0; } else { // 180°より大きければ右旋回 if(360.0f - delta_theta > Alpha) dir = 1; } } } if(dir == 0) { //目標は左方向 pull_L = 20; pull_R = 0; } else if (dir == 1) { //目標は右方向 pull_L = 0; pull_R = 20; } } #ifdef RULE2 // 目標地点との距離が閾値以下だった場合、落下シーケンスへと移行する if(Distance(target_p, p) < BorderDistance) step = 2; #endif break; #ifdef RULE2 // 落下シーケンス case 2: pull_L = PullMax; pull_R = 0; // もし落下中に目標値から離れてしまった場合は、体勢を立て直して再び滑空 // 境界で制御が不安定にならないよう閾値にマージンを取る if(Distance(target_p, p) > BorderDistance+5.0f) step = 1; break; #endif } // 指定された引っ張り量だけ紐を引っ張る if(pull_L < 0) pull_L = 0; else if(pull_L > PullMax) pull_L = PullMax; if(pull_R < 0) pull_R = 0; else if(pull_R > PullMax) pull_R = PullMax; servoL.pulsewidth((ServoMax - ServoMin) * pull_L / PullMax + ServoMin); servoR.pulsewidth((ServoMax - ServoMin) * pull_R / PullMax + ServoMin); } /** 各種ベクトルの初期化を行う関数 * */ void VectorsInit() { //重力ベクトルの初期化 raw_g.SetComp(1, 0.0f); raw_g.SetComp(2, 0.0f); raw_g.SetComp(3, 1.0f); // 機体に固定されたベクトルの初期化 b_f.SetComp(1, 0.0f); b_f.SetComp(2, 0.0f); b_f.SetComp(3, -1.0f); b_u.SetComp(1, 1.0f); b_u.SetComp(2, 0.0f); b_u.SetComp(3, 0.0f); b_l = Cross(b_u, b_f); // 目標座標をベクトルに代入 target_p.SetComp(1, target_x * DEG_TO_RAD); target_p.SetComp(2, target_y * DEG_TO_RAD); } /** マイクロSDカードの初期化を行う関数 * */ void SD_Init() { //SDカード初期化 char filename[15]; int n = Find_last(); if(n < 0) { pc.printf("Could not read a SD Card.\n"); return; } sprintf(filename, "/sd/log%03d.csv", n+1); fp = fopen(filename, "w"); fprintf(fp, "log data\r\n"); xbee.printf("log data\r\n"); } /** コンフィグファイルを読み込む関数 * */ void LoadConfig() { char value[20]; //Read a configuration file from a mbed. if (!cfg.read("/sd/config.txt")) { pc.printf("Config file does not exist\n"); } else { //Get values if (cfg.getValue("target_x", &value[0], sizeof(value))) target_x = atof(value); else { pc.printf("Failed to get value for target_x\n"); } if (cfg.getValue("target_y", &value[0], sizeof(value))) target_y = atof(value); else { pc.printf("Failed to get value for target_y\n"); } } } /** ログファイルの番号の最大値を得る関数 * * @return マイクロSD内に存在するログファイル番号の最大値 */ int Find_last() { int i, n = 0; char c; DIR *dp; struct dirent *dirst; dp = opendir("/sd/"); if (!dp) { pc.printf("Could not open directry.\n"); return -1; } while((dirst = readdir(dp)) != NULL) { if(sscanf(dirst->d_name, "log%03d.csv%c", &i, &c) == 1 && i>n) { n = i; } } closedir(dp); return n; } /** カルマンフィルタの初期化を行う関数 * */ void KalmanInit() { // 重力 { // 誤差共分散行列の値を決める(対角成分のみ) float alpha_R2 = 0.002f; float alpha_Q2 = 0.5f; R2 *= alpha_R2; Q2 *= alpha_Q2; // 観測方程式のヤコビアンの値を設定(時間変化無し) float h2[15] = { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f }; H2.SetComps(h2); } // 地磁気 { // 誤差共分散行列の値を決める(対角成分のみ) float alpha_R1 = 0.002f; float alpha_Q1 = 0.5f; R1 *= alpha_R1; Q1 *= alpha_Q1; // 観測方程式のヤコビアンの値を設定(時間変化無し) float h1[21] = { 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f }; H1.SetComps(h1); } } /** カルマンフィルタの更新を行う関数 * */ void KalmanUpdate() { // 重力 { // ヤコビアンの更新 float f2[25] = { 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, -(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, (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, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f }; F2.SetComps(f2); // 事前推定値の更新 //pri_x2 = F2 * post_x2; float pri_x2_vals[5] = { 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, 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, 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, post_x2.GetComp(4), post_x2.GetComp(5) }; pri_x2.SetComps(pri_x2_vals); // 事前誤差分散行列の更新 pri_P2 = F2 * post_P2 * F2.Transpose() + R2; // カルマンゲインの計算 S2 = Q2 + H2 * pri_P2 * H2.Transpose(); static float det; if((det = S2.Inverse(S_inv2)) >= 0.0f) { pc.printf("E:%.3f\r\n", det); return; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了 } K2 = pri_P2 * H2.Transpose() * S_inv2; // 事後推定値の更新 post_x2 = pri_x2 + K2 * (raw_acc - H2 * pri_x2); // 事後誤差分散行列の更新 post_P2 = (I2 - K2 * H2) * pri_P2; } // 地磁気 { // ヤコビアンの更新 float f1[49] = { 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, -(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, (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, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f }; F1.SetComps(f1); // 事前推定値の更新 //pri_x1 = F1 * post_x1; float pri_x1_vals[7] = { 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, 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, 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, post_x1.GetComp(4), post_x1.GetComp(5), post_x1.GetComp(6), post_x1.GetComp(7) }; pri_x1.SetComps(pri_x1_vals); // 事前誤差分散行列の更新 pri_P1 = F1 * post_P1 * F1.Transpose() + R1; // カルマンゲインの計算 S1 = Q1 + H1 * pri_P1 * H1.Transpose(); static float det; if((det = S1.Inverse(S_inv1)) >= 0.0f) { pc.printf("E:%.3f\r\n", det); return; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了 } K1 = pri_P1 * H1.Transpose() * S_inv1; // 事後推定値の更新 post_x1 = pri_x1 + K1 * (raw_geomag - H1 * pri_x1); // 事後誤差分散行列の更新 post_P1 = (I1 - K1 * H1) * pri_P1; } } /** GPS座標から距離を算出 * @param 座標1(経度、緯度)(rad) * @param 座標2(経度、緯度)(rad) * * @return 2点間の距離(m) */ float Distance(Vector p1, Vector p2) { if(p1.GetDim() != p2.GetDim()) return 0.0f; static float mu_y = (p1.GetComp(2) + p2.GetComp(2)) * 0.5f; static float s_mu_y = sin(mu_y); static float w = sqrt(1 - GPS_SQ_E * s_mu_y * s_mu_y); static float m = GPS_A * (1 - GPS_SQ_E) / (w * w * w); static float n = GPS_A / w; static float d1 = m * (p1.GetComp(2) - p2.GetComp(2)); static float d2 = n * cos(mu_y) * (p1.GetComp(1) - p2.GetComp(1)); return sqrt(d1 * d1 + d2 * d2); } /** 投下を検知する関数 * * @return 投下されたかどうか(true=投下 false=未投下 * */ bool Thrown() { static int opt_count = 0; static int g_count = 0; static int para_count = 0; if(optSensor.read_u16() > BorderOpt) opt_count++; else opt_count = 0; if(vrt_acc < BorderGravity) g_count++; else g_count = 0; if((int)paraSensor == BorderParafoil) para_count++; else para_count = 0; if(opt_count >= MaxCount || g_count >= MaxCount || para_count >= MaxCount) { return true; } return false; } /** データ取得&更新関数 * */ void DataUpdate() { // センサーの値を更新 mpu.read(); hmc.read(); for(int i=0; i<3; i++) { raw_acc.SetComp(i+1, (float)mpu.data.value.acc[i] * ACC_LSB_TO_G); raw_gyro.SetComp(i+1, (float)mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD); raw_geomag.SetComp(i+1, (float)hmc.data.value[i] * MAG_LSB_TO_GAUSS); } Vector delta_g = Cross(raw_gyro, raw_g); // Δg = ω × g raw_g = 0.9f * (raw_g - delta_g * dt) + 0.1f * raw_acc.Normalize(); // 相補フィルタ raw_g = raw_g.Normalize(); KalmanUpdate(); // LPS25Hによる気圧の取得は10Hz lps_cnt = (lps_cnt+1)%10; if(lps_cnt == 0) { raw_press = (float)lps.pressure() * PRES_LSB_TO_HPA; } if(INT_flag != FALSE) { g = raw_g; for(int i=0; i<3; i++) { geomag.SetComp(i+1, post_x1.GetComp(i+1)); } acc = raw_acc; gyro = raw_gyro; press = raw_press; vrt_acc = raw_acc * raw_g; } } /* ------------------------------ 割り込み関数 ------------------------------ */ /* ------------------------------ デバッグ用関数 ------------------------------ */ void toString(Matrix& m) { for(int i=0; i<m.GetRow(); i++) { for(int j=0; j<m.GetCol(); j++) { pc.printf("%.6f\t", m.GetComp(i+1, j+1)); } pc.printf("\r\n"); } } void toString(Vector& v) { for(int i=0; i<v.GetDim(); i++) { pc.printf("%.6f\t", v.GetComp(i+1)); } pc.printf("\r\n"); }