Diff: main.cpp

Revision:

9:6d4578dcc1ed

Parent:

8:602865d8fca3

Child:

10:8ee11e412ad7

diff -r 602865d8fca3 -r 6d4578dcc1ed main.cpp
--- a/main.cpp	Fri Jun 12 15:28:05 2015 +0000
+++ b/main.cpp	Mon Jun 15 10:40:18 2015 +0000
@@ -14,9 +14,14 @@
 #define TRUE    1
 #define FALSE   0
 
+#define x       1
+#define y       2
+#define z       3
+
 const float dt = 0.1f;              // 割り込み周期(s)
 const float ServoMax = 0.0023f;     // サーボの最大パルス長(s)
 const float ServoMin = 0.0006f;     // サーボの最小パルス長(s)
+const float PullMax  = 25.0f;       // 引っ張れる紐の最大量(mm)
 /********** private macro     **********/
 
 /********** private typedef   **********/
@@ -50,10 +55,26 @@
 
 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);                  // 地磁気ベクトル
 
-int pull_L = 0;                 // 左サーボの引っ張り量(mm：0~30)
-int pull_R = 0;                 // 右サーボの引っ張り量(mm：0~30)
+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）
+float pitch = 0.0f;             // 本体のピッチ角（deg）
+float roll = 0.0f;              // 本体のロール角（deg）
 
 /* ----- Kalman Filter ----- */
 Vector pri_x(6);
@@ -72,9 +93,10 @@
 char data[512] = {};
 
 /********** private functions **********/
-void KalmanInit();                  // カルマンフィルタ初期化
-void KalmanUpdate();                // カルマンフィルタ更新
-void INT_func();                    // 割り込み用関数
+void KalmanInit();                      // カルマンフィルタ初期化
+void KalmanUpdate();                    // カルマンフィルタ更新
+float distance(Vector p1, Vector p2);   // 緯度・経度の差から2点間の距離を算出(m)
+void INT_func();                        // 割り込み用関数
 void toString(Matrix& m);
 void toString(Vector& v);
 
@@ -102,6 +124,15 @@
     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);
+    
     /* ---------- ↓↓↓ ここからメインループ ↓↓↓ ---------- */
     while(1) {
         timer.stop();
@@ -109,14 +140,44 @@
         timer.start();
         // 0.1秒おきにセンサーの出力をpcへ出力
         myled = 1; // LED is ON
-        wait(0.05f); // 50 ms
-        myled = 0; // LED is OFF
-        
-        pull_L = (pull_L+5)%30;
-        pull_R = (pull_R+5)%30;
         
         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)成分
+            
+            yaw = atan2(R_11, -R_12) * RAD_TO_DEG;
+            roll = atan2(R_33, -R_23) * RAD_TO_DEG;
+            pitch = atan2(R_11/cos(yaw), R_13) * RAD_TO_DEG;
+            
+            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 {                                        // 更新されていなかったら
+                
+            }
+            
+            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;
         
@@ -129,20 +190,23 @@
         
         INT_flag = TRUE;            // 割り込み許可
         
-        pc.printf("time: %.3f[s]\r\n", (float)timer.read_ms()/1000.0f);
-        
         // 制御ルーチン
-        /*{
-            servoL.pulsewidth((ServoMax-ServoMin) * pull_L / 30.0f + ServoMin);
-            servoR.pulsewidth((ServoMax-ServoMin) * pull_R / 30.0f + ServoMin);
+        {
+            //pull_L = (pull_L+5)%30;
+            //pull_R = (pull_R+5)%30;
+            pull_L = 0;
+            pull_R = 30;
+            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);
         
-        // ループはきっかり1秒ごと
-        while(timer.read_ms() < 1000);
+        pc.printf("time: %.3f[s]\r\n", (float)timer.read_ms()/1000.0f);
         
     }
     
@@ -212,6 +276,22 @@
     post_P = (I - K * H) * pri_P;
 }
 
+float distance(Vector p1, Vector p2) {
+    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));
+    
+    return sqrt(d1 * d1 + d2 * d2);
+}
+
+/* -------------------- 割り込み関数 -------------------- */
+
 void INT_func() {
     // センサーの値を更新
     mpu.read();
@@ -248,6 +328,8 @@
     }
 }
 
+/* -------------------- デバッグ用関数 -------------------- */
+
 void toString(Matrix& m) {
     
     for(int i=0; i<m.GetRow(); i++) {