Diff: main.cpp

Revision:

17:03b45055ca05

Parent:

15:d14d385d37e2

Child:

19:ad8ff2de68f5

--- a/main.cpp	Sat Jun 20 16:39:11 2015 +0000
+++ b/main.cpp	Sun Jun 21 15:29:37 2015 +0000
@@ -58,10 +58,12 @@
 
 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);              // 地磁気(?)
@@ -133,9 +135,11 @@
 
 
 /****************************** private functions ******************************/
+void    SD_Init();                          // SDカード初期化
+void    VectorsInit();                      // 各種ベクトルの初期化
+void    KalmanInit();                       // カルマンフィルタ初期化
 void    LoadConfig();                       // config読み取り
 int     Find_last();                        // SDカード初期化用関数
-void    KalmanInit();                       // カルマンフィルタ初期化
 void    KalmanUpdate();                     // カルマンフィルタ更新
 float   Distance(Vector p1, Vector p2);     // 緯度・経度の差から2点間の距離を算出(m)
 bool    thrown();                           // 投下されたかどうかを判断する
@@ -161,44 +165,20 @@
     //Config読み取り
     LoadConfig();
 
-    //SDカード初期化
-    FILE *fp;
-    char filename[15];
-    int n = Find_last();
-    if(n < 0) {
-        pc.printf("Could not read a SD Card.\n");
-        return 0;
-    }
-    sprintf(filename, "/sd/log%03d.csv", n+1);
-    fp = fopen(filename, "w");
-    fprintf(fp, "log data\r\n");
-    xbee.printf("log data\r\n");
-
-    servoL.period(0.020f);                  // サーボの信号の周期は20ms
-    servoR.period(0.020f);
+    // SDカード初期化
+    SD_Init();
 
     //カルマンフィルタ初期化
     KalmanInit();
 
+    // 各種ベクトルの初期化
+    VectorsInit();
+    
     INT_timer.attach(&INT_func, dt);        // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
-
-    //重力ベクトルの初期化
-    raw_g.SetComp(1, 0.0f);
-    raw_g.SetComp(2, 0.0f);
-    raw_g.SetComp(3, 1.0f);
+    
+    servoL.period(0.020f);                  // サーボの信号の周期は20ms
+    servoR.period(0.020f);
 
-    // 機体に固定されたベクトルの初期化
-    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);
 
     /* ------------------------------ ↓↓↓ ここからメインループ ↓↓↓ ------------------------------ */
     while(1) {
@@ -214,8 +194,6 @@
             gms.read();                                     // GPSデータ取得
             UTC_t = (int)gms.time;
 
-
-
             // 参照座標系の基底を求める
             r_u = g;
             r_f = geomag.GetPerpCompTo(g).Normalize();
@@ -242,8 +220,6 @@
             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);
@@ -259,7 +235,7 @@
             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,%d\r\n",
+            sprintf(data, "%.3f,%.3f,%.3f, %.3f,%.6f,%.6f, %.3f,%.3f,%.3f, %.3f,%d\r\n",
                     yaw, pitch, roll,
                     press, gms.longitude, gms.latitude,
                     sv, lv, vrt_acc,
@@ -353,7 +329,7 @@
 
                         }
                     }
-
+                    
 #ifdef RULE2
                     // 目標地点との距離が閾値以下だった場合、落下シーケンスへと移行する
                     if(Distance(target_p, p) < BorderDistance) step = 2;
@@ -364,7 +340,7 @@
 #ifdef RULE2
                 // 落下シーケンス
                 case 2:
-                    pull_L = 25;
+                    pull_L = PullMax;
                     pull_R = 0;
 
                     // もし落下中に目標値から離れてしまった場合は、体勢を立て直して再び滑空
@@ -383,21 +359,62 @@
             servoL.pulsewidth((ServoMax - ServoMin) * pull_L / PullMax + ServoMin);
             servoR.pulsewidth((ServoMax - ServoMin) * pull_R / PullMax + ServoMin);
 
-
-
         }
 
         myled = 0; // LED is OFF
 
         // ループはきっかり0.2秒ごと
         while(timer.read_ms() < 200);
-
-
+        
     }
 
     /* ------------------------------ ↑↑↑ ここまでメインループ ↑↑↑ ------------------------------ */
 }
 
+/** 各種ベクトルの初期化を行う関数
+ *
+ */
+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カード初期化
+    FILE *fp;
+    char filename[15];
+    int n = Find_last();
+    if(n < 0) {
+        pc.printf("Could not read a SD Card.\n");
+        return 0;
+    }
+    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];
@@ -417,6 +434,10 @@
     }
 }
 
+/** ログファイルの番号の最大値を得る関数
+ *
+ * @return マイクロSD内に存在するログファイル番号の最大値
+ */
 int Find_last()
 {
     int i, n = 0;
@@ -437,6 +458,9 @@
     return n;
 }
 
+/** カルマンフィルタの初期化を行う関数
+ *
+ */
 void KalmanInit()
 {
     // 重力
@@ -476,6 +500,9 @@
     }
 }
 
+/** カルマンフィルタの更新を行う関数
+ *
+ */
 void KalmanUpdate()
 {
     // 重力
@@ -567,24 +594,17 @@
 
         // 事後推定値の更新
         post_x1 = pri_x1 + K1 * (raw_geomag - H1 * pri_x1);
-        // 地磁気ベクトルの大きさに拘束条件を与える
-        /*{
-            Vector gm(3);
-            gm.SetComp(1, post_x1.GetComp(1));
-            gm.SetComp(2, post_x1.GetComp(2));
-            gm.SetComp(3, post_x1.GetComp(3));
-
-            gm = MAG_MAGNITUDE * gm.Normalize();
-
-            post_x1.SetComp(1, gm.GetComp(1));
-            post_x1.SetComp(2, gm.GetComp(2));
-            post_x1.SetComp(3, gm.GetComp(3));
-        }*/
         // 事後誤差分散行列の更新
         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;
@@ -604,7 +624,7 @@
  *
  * @return 投下されたかどうか（true=投下　false=未投下
  *
-*/
+ */
 bool thrown()
 {
     static int opt_count = 0;