MPUとHMCでうごくかもver
Dependencies: ConfigFile SDFileSystem mbed
Fork of LAURUS_program by
Diff: main.cpp
- Revision:
- 25:4c72d7420d8a
- Parent:
- 24:8838be99cec3
- Child:
- 26:6e09df57ee91
--- a/main.cpp Thu Jun 25 17:40:18 2015 +0000 +++ b/main.cpp Fri Jun 26 15:09:01 2015 +0000 @@ -12,20 +12,16 @@ #include "ConfigFile.h" /****************************** private define ******************************/ -#define LEFT 0 -#define RIGHT 1 -#define NEUTRAL 2 - #define RULE1 //#define RULE2 //#define RULE3 //#define SERVO_DEBUG //#define DIRECTION_DEBUG + + #ifdef DIRECTION_DEBUG const float TargetDirection = 90.0f; // 真西に飛ぶ #endif - - const float dt = 0.01f; // 割り込み周期(s) const float ServoMax = 0.0046f; // サーボの最大パルス長(s) const float ServoMin = 0.0012f; // サーボの最小パルス長(s) @@ -40,6 +36,8 @@ const float Beta = 60.0f; // 目標方向と自分の進行方向との間に取るべき角度差(deg)(制御則3の定数 const float BorderDistance = 10.0f; // 落下制御に入るための目標値との距離の閾値(m)(制御則2の定数 +enum Direction {LEFT, RIGHT, NEUTRAL}; + /****************************** private macro ******************************/ /****************************** private typedef ******************************/ /****************************** private variables ******************************/ @@ -62,9 +60,10 @@ DigitalIn paraSensor(PB_0); // パラフォイルに繋がる(予定)の物理スイッチ Ticker ticker; // 割り込みタイマー Timer timer; // 時間計測用タイマー +Direction dir = NEUTRAL; // 旋回方向 int lps_cnt = 0; // 気圧センサ読み取りカウント -bool INT_flag = true; // 割り込み可否フラグ +bool INT_flag = true; // 割り込み可否フラグ /* こちらの変数群はメインループでは参照しない */ Vector raw_acc(3); // 加速度(m/s^2) 生 Vector raw_gyro(3); // 角速度(deg/s) 生 @@ -83,7 +82,8 @@ Vector raw_g(3); // 重力ベクトル 生 Vector g(3); // 重力ベクトル Vector target_p(2); // 目標情報(経度、緯度)(rad) -Vector p(2); // 位置情報(経度, 緯度)(rad) +Vector p(2); // 現在の位置情報(補完含む)(経度, 緯度)(rad) +Vector new_p(2); // 最新の位置情報(経度, 緯度)(rad) Vector pre_p(2); // 過去の位置情報(経度, 緯度)(rad) int UTC_t = 0; // UTC時刻 int pre_UTC_t = 0; // 前のUTC時刻 @@ -107,9 +107,10 @@ int step = 0; // シーケンス制御のステップ int count = 0; // 安定滑空までの時間測定用カウンター -int dir = 0; // 旋回方向(0:左 1:右) char data[512] = {}; // 送信データ用配 int loopTime = 0; // 1ループに掛かる時間(デバッグ用 +float sv = 0.0f; // サーボ電源電圧 +float lv = 0.0f; // ロジック電源電圧 /** config.txt ** * #から始めるのはコメント行 @@ -161,13 +162,24 @@ void toString(Matrix& m); // デバッグ用 void toString(Vector& v); // デバッグ用 +/** 小さい値を返す関数 + * @param a: 1つ目の入力値 + * @param b: 2つ目の入力値 + * + * @return a,bのうち、小さい方の値 + */ inline float min(float a, float b) { return ((a > b) ? b : a); } -inline float Height(float press, float temp) { - return (153.8f * (pow((p0/press),0.1902f)-1.0f)*(temp+273.15f)); +/** 気圧の変化から高度を計算する関数 + * @param press: 現在の気圧 + * + * @return height: 現在の基準点からの高度 + */ +inline float Height(float press) { + return (153.8f * (pow((p0/press),0.1902f)-1.0f)*(25.0f+273.15f)); } /****************************** main function ******************************/ @@ -230,8 +242,8 @@ ControlRoutine(); #endif - float sv = (float)servoVcc.read_u16() * ADC_LSB_TO_V * 2.0f; // サーボ電源電圧 - float lv = (float)logicVcc.read_u16() * ADC_LSB_TO_V * 2.0f; // ロジック電源電圧 + sv = (float)servoVcc.read_u16() * ADC_LSB_TO_V * 2.0f; // サーボ電源電圧 + lv = (float)logicVcc.read_u16() * ADC_LSB_TO_V * 2.0f; // ロジック電源電圧 // 指定された引っ張り量だけ紐を引っ張る if(pull_L < 0) pull_L = 0; @@ -243,12 +255,11 @@ servoR.pulsewidth((ServoMax - ServoMin) * pull_R / (float)PullMax + ServoMin); // データをmicroSDに保存し、XBeeでPCへ送信する - sprintf(data, "%d, %.1f,%.1f,%.1f, %.3f,%.5f,%.5f, %.3f,%.3f,%.3f, %.3f,%d, %d,%d\r\n", + sprintf(data, "%d, %.1f,%.1f,%.1f, %.3f,%.5f,%.5f, %.3f,%.3f,%.1f, %d, %d,%d\r\n", UTC_t, yaw, pitch, roll, press, gms.longitude, gms.latitude, - vrt_acc, temp, height, - Distance(target_p, p), optSensor.read_u16(), - pull_R, pull_L); + vrt_acc, height, Distance(target_p, p), + optSensor.read_u16(), pull_R, pull_L); INT_flag = true; // 割り込み許可 @@ -274,6 +285,13 @@ */ void DataProcessing() { + static float R_11; + static float R_12; + static float r_cos; + static float r_sin; + static float p_cos; + static float p_sin; + gms.read(); // GPSデータ取得 UTC_t = (int)gms.time; @@ -286,35 +304,46 @@ // オイラー角はヨー・ピッチ・ロールの順に回転したものとする // 各オイラー角を求めるのに回転行列の全成分は要らないので、一部だけ計算する。 - 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)成分 + R_11 = r_f * b_f; // 回転行列の(1,1)成分 + R_12 = r_f * b_l; // 回転行列の(1,2)成分 #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; + r_cos = g.GetPerpCompTo(b_f).Normalize() * b_u; + r_sin = Cross(g.GetPerpCompTo(b_f).Normalize(), b_u) * b_f; + if(r_sin > 0.0f) { + roll = acos(r_cos) * RAD_TO_DEG; + } else { + roll = -acos(r_cos) * RAD_TO_DEG; + } + + p_cos = g.GetPerpCompTo(b_l).Normalize() * b_u; + p_sin = Cross(g.GetPerpCompTo(b_l).Normalize(), b_u) * b_l; + if(p_sin > 0.0f) { + pitch = acos(p_cos) * RAD_TO_DEG; + } else { + pitch = -acos(p_cos) * 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); - + pre_p = new_p; + new_p.SetComp(1, gms.longitude * DEG_TO_RAD); + new_p.SetComp(2, gms.latitude * DEG_TO_RAD); + p = new_p; + pre_UTC_t = UTC_t; + } else { // 更新されていなかったら - // GPSの補完処理を追加予定 + p += 0.2f * (new_p - pre_p); } - height = Height(press, temp); + height = Height(press); - pre_p = p; - pre_UTC_t = UTC_t; } @@ -324,6 +353,13 @@ */ void ControlRoutine() { + static float target_lng; + static float target_lat; + static float target_X; + static float target_Y; + static float theta; + static float delta_theta; + switch(step) { // 投下&安定滑空シーケンス @@ -355,13 +391,13 @@ } else { /* いずれも地球を完全球体と仮定 */ - float target_lng = target_x * DEG_TO_RAD; - float target_lat = target_y * DEG_TO_RAD; + target_lng = target_x * DEG_TO_RAD; + 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 ) * RAD_TO_DEG; - float delta_theta = 0.0f; + target_Y = cos( target_lat ) * sin( target_lng - p.GetComp(1) ); + target_X = cos( p.GetComp(2) ) * sin( target_lat ) - sin( p.GetComp(2) ) * cos( target_lat ) * cos( target_lng - p.GetComp(1) ); + theta = -atan2f( target_Y, target_X ) * RAD_TO_DEG; + delta_theta = 0.0f; #ifdef DIRECTION_DEBUG theta = TargetDirection; #endif @@ -467,6 +503,7 @@ p0 = 0; for(int i=0; i<10; i++) { p0 += (float)lps.pressure() * PRES_LSB_TO_HPA; + wait(0.1f); } p0 /= 10.0f; } @@ -690,15 +727,23 @@ */ float Distance(Vector p1, Vector p2) { + static float mu_y; + static float s_mu_y; + static float w; + static float m; + static float n; + static float d1; + static float d2; + if(p1.GetDim() != p2.GetDim()) return 0.0f; - float mu_y = (p1.GetComp(2) + p2.GetComp(2)) * 0.5f; - float s_mu_y = sin(mu_y); - float w = sqrt(1 - GPS_SQ_E * s_mu_y * s_mu_y); - float m = GPS_A * (1 - GPS_SQ_E) / (w * w * w); - float n = GPS_A / w; - float d1 = m * (p1.GetComp(2) - p2.GetComp(2)); - float d2 = n * cos(mu_y) * (p1.GetComp(1) - p2.GetComp(1)); + mu_y = (p1.GetComp(2) + p2.GetComp(2)) * 0.5f; + s_mu_y = sin(mu_y); + w = sqrt(1 - GPS_SQ_E * s_mu_y * s_mu_y); + m = GPS_A * (1 - GPS_SQ_E) / (w * w * w); + n = GPS_A / w; + d1 = m * (p1.GetComp(2) - p2.GetComp(2)); + d2 = n * cos(mu_y) * (p1.GetComp(1) - p2.GetComp(1)); return sqrt(d1 * d1 + d2 * d2); }