MPUとHMCでうごくかもver
Dependencies: ConfigFile SDFileSystem mbed
Fork of LAURUS_program by
Diff: main.cpp
- Revision:
- 14:f85cb5340cb8
- Parent:
- 13:df1e8a650185
- Child:
- 15:d14d385d37e2
--- a/main.cpp Fri Jun 19 01:08:35 2015 +0000 +++ b/main.cpp Fri Jun 19 15:56:52 2015 +0000 @@ -12,19 +12,25 @@ #include "BufferedSerial.h" #include "ConfigFile.h" -/********** private define **********/ +/****************************** private define ******************************/ #define TRUE 1 #define FALSE 0 -const float dt = 0.01f; // 割り込み周期(s) -const float ServoMax = 0.0023f; // サーボの最大パルス長(s) -const float ServoMin = 0.0006f; // サーボの最小パルス長(s) -const float PullMax = 25.0f; // 引っ張れる紐の最大量(mm) -/********** private macro **********/ +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; // 目標方向と自分の進行方向との差の閾値(制御則1の定数 -/********** private typedef **********/ - -/********** private variables **********/ +/****************************** private macro ******************************/ +/****************************** private typedef ******************************/ +/****************************** private variables ******************************/ DigitalOut myled(LED1); // デバッグ用LEDのためのデジタル出力 I2C i2c(PB_9, PB_8); // I2Cポート MPU6050 mpu(&i2c); // 加速度・角速度センサ @@ -35,48 +41,55 @@ GMS6_CR6 gms(&gps, &pc); // GPS SDFileSystem sd(PB_5, PB_4, PB_3, PB_10, "sd"); // microSD BufferedSerial xbee(PA_9, PA_10, PC_1); // Xbee -ConfigFile cfg; //ConfigFile +ConfigFile cfg; // ConfigFile PwmOut servoL(PB_6), servoR(PC_7); // サーボ用PWM出力 -AnalogIn optSensor(PC_0); // 照度センサ用アナログ入力 +AnalogIn optSensor(PC_0); // 照度センサ用アナログ入力 AnalogIn servoVcc(PA_0); // バッテリー電圧監視用アナログ入力(サーボ用) AnalogIn logicVcc(PA_1); // バッテリー電圧監視用アナログ入力(ロジック用) +DigitalIn paraSensor(PB_0); // パラフォイルに繋がる(予定)の物理スイッチ Ticker INT_timer; // 割り込みタイマー +Timer timer; // 時間計測用タイマー -int lps_cnt = 0; // 気圧センサ読み取りカウント +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_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 p(2); // 位置情報(経度, 緯度) -Vector pre_p(2); // 過去の位置情報(経度, 緯度) -int UTC_t = 0; // UTC時刻 -int pre_UTC_t = 0; // 前のUTC時刻 -//Vector n(3); // 地磁気ベクトル +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 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軸) +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) -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 yaw = 0.0f; // 本体のヨー角(deg)z軸周り -float pitch = 0.0f; // 本体のピッチ角(deg)y軸周り -float roll = 0.0f; // 本体のロール角(deg)x軸周り +float vrt_acc = 0.0f; // 鉛直方向の加速度成分(落下検知に使用) -float vrt_acc = 0.0f; // 鉛直方向の加速度成分(落下検知に使用) +int step = 0; // シーケンス制御のステップ +int count = 0; // 時間測定用カウンター +int dir = 0; // 旋回方向(0:左 1:右) +char data[512] = {}; // 送信データ用配列 /** config.txt ** * #から始めるのはコメント行 @@ -86,8 +99,9 @@ */ float target_x, target_y; -/* ----- Kalman Filter ----- */ +/* ---------- Kalman Filter ---------- */ // 地磁気ベクトル用 +// ジャイロのz軸周りのバイアスも推定 Vector pri_x1(7); Matrix pri_P1(7, 7); Vector post_x1(7); @@ -99,7 +113,7 @@ Matrix S1(3, 3), S_inv1(3, 3); // 重力ベクトル用 -// ジャイロのバイアスも同時に推定する +// ジャイロのx軸、y軸周りのバイアスも同時に推定 Vector pri_x2(5); Matrix pri_P2(5, 5); Vector post_x2(5); @@ -109,38 +123,40 @@ Matrix I2(5, 5); Matrix K2(5, 3); Matrix S2(3, 3), S_inv2(3, 3); -/* ----- ------------- ----- */ +/* ---------- ------------- ---------- */ -Timer timer; - -char data[512] = {}; -/********** private functions **********/ -void LoadConfig(); // config読み取り -int find_last(); // SDカード初期化用関数 -void KalmanInit(); // カルマンフィルタ初期化 -void KalmanUpdate(); // カルマンフィルタ更新 -float distance(Vector p1, Vector p2); // 緯度・経度の差から2点間の距離を算出(m) -void INT_func(); // 割り込み用関数 -void toString(Matrix& m); -void toString(Vector& v); +/****************************** private functions ******************************/ +void LoadConfig(); // config読み取り +int Find_last(); // SDカード初期化用関数 +void KalmanInit(); // カルマンフィルタ初期化 +void KalmanUpdate(); // カルマンフィルタ更新 +float Distance(Vector p1, Vector p2); // 緯度・経度の差から2点間の距離を算出(m) +bool thrown(); // 投下されたかどうかを判断する +void INT_func(); // 割り込み用関数 +void toString(Matrix& m); // デバッグ用 +void toString(Vector& v); // デバッグ用 -/********** main function **********/ +inline float min(float a, float b) { + return ((a > b) ? b : a); +} + +/****************************** main function ******************************/ int main() { - i2c.frequency(400000); // I2Cの通信速度を400kHzに設定 + i2c.frequency(400000); // I2Cの通信速度を400kHzに設定 if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!"); // mpu6050初期化 if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!"); // hmc5883l初期化 - /* + //Config読み取り LoadConfig(); //SDカード初期化 FILE *fp; char filename[15]; - int n = find_last(); + int n = Find_last(); if(n < 0){ pc.printf("Could not read a SD Card.\n"); return 0; @@ -149,14 +165,14 @@ fp = fopen(filename, "w"); fprintf(fp, "log data\r\n"); xbee.printf("log data\r\n"); - */ - servoL.period(0.020f); // サーボの信号の周期は20ms + + servoL.period(0.020f); // サーボの信号の周期は20ms servoR.period(0.020f); //カルマンフィルタ初期化 KalmanInit(); - INT_timer.attach(&INT_func, dt); // 割り込み有効化(Freq = 0.01fなので、10msおきの割り込み) + INT_timer.attach(&INT_func, dt); // 割り込み有効化(Freq = 0.01fなので、10msおきの割り込み) //重力ベクトルの初期化 raw_g.SetComp(1, 0.0f); @@ -172,79 +188,81 @@ 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); + + /* ------------------------------ ↓↓↓ ここからメインループ ↓↓↓ ------------------------------ */ while(1) { timer.stop(); timer.reset(); timer.start(); - // 0.1秒おきにセンサーの出力をpcへ出力 - myled = 1; // LED is ON + myled = 1; // LED is ON - // データ処理 + /******************************* データ処理 *******************************/ { - INT_flag = FALSE; // 割り込みによる変数書き換えを阻止 + 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)成分 + 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)成分 yaw = atan2(-R_12, R_11) * RAD_TO_DEG - MAG_DECLINATION; roll = atan2(-R_23, R_33) * RAD_TO_DEG; pitch = asin(R_13) * RAD_TO_DEG; - pc.printf("%.3f, %.3f, %.3f\r\n", - //geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3)); - //yaw, pitch, roll); - post_x2.GetComp(4), - post_x2.GetComp(5), - post_x1.GetComp(7)); + 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); + 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; + 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, "%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%d\r\n", - g.GetComp(1), g.GetComp(2), g.GetComp(3), - geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3), + sprintf(data, "%.3f,%.3f,%.3f, %.3f,%.3f,%.3f, %.3f,%.3f,%.3f, %.3f,%d\r\n", + yaw, pitch, roll, press, gms.longitude, gms.latitude, - sv, lv, optSensor.read_u16()); - //fprintf(fp, data); - //fflush(fp); - //xbee.printf(data); + sv, lv, vrt_acc, + Distance(target_p, p), optSensor.read_u16()); + fprintf(fp, data); + fflush(fp); + xbee.printf(data); INT_flag = TRUE; // 割り込み許可 } - // 制御ルーチン + /******************************* 制御ルーチン *******************************/ { - if(fabs(roll) > 40.0f) { + if(fabs(roll) > BorderSpiral) { // スパイラル回避行動 if(roll > 0) { pull_L = PullMax; @@ -255,15 +273,80 @@ } } else { - } + 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; - //pull_L = (pull_L+5)%PullMax; - //pull_R = (pull_R+5)%PullMax; - //pull_L = 0; - //pull_R = 30; - - - + // 制御ルーチン + case '1': + /* いずれも地球を完全球体と仮定 */ + 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; + delta_theta = fabs(theta - yaw); + if(delta_theta < 180.0f) { + if(delta_theta > Alpha) dir = 0; + } else { + 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 { + theta -= 360.0f; + delta_theta = fabs(theta - yaw); + if(delta_theta < 180.0f) { + if(delta_theta > Alpha) dir = 1; + } else { + if(360.0f - delta_theta > Alpha) dir = 0; + } + } + } + + + if(dir == 0) { //目標は左方向 + + pull_L = 20; + pull_R = 0; + + } else if (dir == 1) { //目標は右方向 + + pull_L = 0; + pull_R = 20; + + } + + break; + } + + } + // 指定された引っ張り量だけ紐を引っ張る if(pull_L < 0) pull_L = 0; else if(pull_L > PullMax) pull_L = PullMax; @@ -273,13 +356,11 @@ servoL.pulsewidth((ServoMax - ServoMin) * pull_L / PullMax + ServoMin); servoR.pulsewidth((ServoMax - ServoMin) * pull_R / PullMax + ServoMin); + + } myled = 0; // LED is OFF - /*pc.printf("%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n", - post_x.GetComp(4), post_x.GetComp(5), post_x.GetComp(6), - yaw, pitch, roll, - geomag.GetNorm());*/ // ループはきっかり0.2秒ごと while(timer.read_ms() < 200); @@ -287,8 +368,7 @@ } - /* ---------- ↑↑↑ ここまでメインループ ↑↑↑ ---------- */ - //fclose(fp); + /* ------------------------------ ↑↑↑ ここまでメインループ ↑↑↑ ------------------------------ */ } void LoadConfig(){ @@ -309,7 +389,7 @@ } } -int find_last() { +int Find_last() { int i, n = 0; char c; DIR *dp; @@ -474,7 +554,7 @@ } } -float distance(Vector p1, Vector p2) { +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; @@ -488,7 +568,32 @@ 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 INT_func() { // センサーの値を更新 @@ -501,7 +606,7 @@ raw_geomag.SetComp(i+1, (float)hmc.data.value[i] * MAG_LSB_TO_GAUSS); } - Vector delta_g = Cross(raw_gyro, raw_g); // Δg = ω × g + 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(); @@ -512,7 +617,6 @@ if(lps_cnt == 0) { raw_press = (float)lps.pressure() * PRES_LSB_TO_HPA; } - //pc.printf("%d(us)\r\n", timer.read_us()); if(INT_flag != FALSE) { g = raw_g; @@ -528,7 +632,7 @@ } } -/* -------------------- デバッグ用関数 -------------------- */ +/* ------------------------------ デバッグ用関数 ------------------------------ */ void toString(Matrix& m) {