Diff: main.cpp

Revision:

27:a26ff85bba23

Parent:

26:6e09df57ee91

Child:

28:d993f3bbe302

--- a/main.cpp	Sun Jun 28 05:48:46 2015 +0000
+++ b/main.cpp	Sun Jun 28 15:42:26 2015 +0000
@@ -12,14 +12,14 @@
 #include "ConfigFile.h"
 
 /****************************** private define    ******************************/
-#define RULE1
+//#define RULE1
 //#define RULE2
 //#define RULE3
 //#define SERVO_DEBUG
-//#define DIRECTION_DEBUG
+#define DIRECTION_DEBUG
 
 #ifdef DIRECTION_DEBUG
-const float TargetDirection = 90.0f;        // 真西に飛ぶ
+const float TargetDirection = -90.0f;        // 真西に飛ぶ
 #endif
 const float dt              = 0.01f;        // 割り込み周期(s)
 const float ServoMax        = 0.0046f;      // サーボの最大パルス長(s)
@@ -52,7 +52,7 @@
 FILE *          fp;                                 // ログファイルのポインタ
 BufferedSerial  xbee(PA_9, PA_10, PC_1);            // Xbee
 ConfigFile      cfg;                                // ConfigFile
-PwmOut          servoL(PC_7), servoR(PB_6);         // サーボ用PWM出力
+PwmOut          servoL(PB_6), servoR(PC_7);         // サーボ用PWM出力
 AnalogIn        optSensor(PC_0);                    // 照度センサ用アナログ入力
 AnalogIn        servoVcc(PA_1);                     // バッテリー電圧監視用アナログ入力(サーボ用)
 AnalogIn        logicVcc(PA_0);                     // バッテリー電圧監視用アナログ入力(ロジック用)
@@ -86,6 +86,7 @@
 Vector  pre_p(2);               // 過去の位置情報(経度, 緯度)（rad）
 int     UTC_t = 0;              // UTC時刻
 int     pre_UTC_t = 0;          // 前のUTC時刻
+int     ss = 0.0f;              // 時刻の秒数の小数部分
 
 Vector  b_f(3);                 // 機体座標に固定された、機体前方向きのベクトル（x軸）
 Vector  b_u(3);                 // 機体座標に固定された、機体上方向きのベクトル（z軸）
@@ -258,12 +259,12 @@
         if(pull_R < 0) pull_R = 0;
         else if(pull_R > PullMax) pull_R = PullMax;
     
-        servoL.pulsewidth((ServoMax - ServoMin) * pull_L / (float)PullMax + ServoMin);
+        servoL.pulsewidth((ServoMax - ServoMin) * (PullMax - pull_L) / (float)PullMax + ServoMin);
         servoR.pulsewidth((ServoMax - ServoMin) * pull_R / (float)PullMax + ServoMin);
         
         // データをmicroSDに保存し、XBeeでPCへ送信する
-        sprintf(data, "%d, %.1f,%.1f,%.1f, %.3f,%.6f,%.6f, %.3f,%.3f,%.1f, %d, %d,%d\r\n",
-                UTC_t, yaw, pitch, roll,
+        sprintf(data, "%d.%d, %.1f,%.1f,%.1f, %.3f,%.6f,%.6f, %.3f,%.3f,%.1f, %d, %d,%d\r\n",
+                UTC_t + 90000, ss, yaw, pitch, roll,
                 press, gms.longitude, gms.latitude,
                 vrt_acc, height, Distance(target_p, p), 
                 optSensor.read_u16(), pull_R, pull_L);
@@ -292,12 +293,12 @@
  */
 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;
+    static float R_11;                              // 回転行列(1,1)成分
+    static float R_12;                              // 回転行列(1,2)成分
+    static float r_cos;                             // ロール角のcos値
+    static float r_sin;                             // ロール角のsin値
+    static float p_cos;                             // ピッチ角のcos値
+    static float p_sin;                             // ピッチ角のsin値
     
     gms.read();                                     // GPSデータ取得
     UTC_t = (int)gms.time;
@@ -344,9 +345,10 @@
         new_p.SetComp(2, gms.latitude * DEG_TO_RAD);
         p = new_p;
         pre_UTC_t = UTC_t;
-        
+        ss = 0;
     } else {                                        // 更新されていなかったら
         p += 0.2f * (new_p - pre_p);
+        ss += 2;
     }
 
     height = Height(press);