Diff: main.cpp

Revision:

26:6e09df57ee91

Parent:

25:4c72d7420d8a

Child:

27:a26ff85bba23

--- a/main.cpp	Fri Jun 26 15:09:01 2015 +0000
+++ b/main.cpp	Sun Jun 28 05:48:46 2015 +0000
@@ -18,7 +18,6 @@
 //#define SERVO_DEBUG
 //#define DIRECTION_DEBUG
 
-
 #ifdef DIRECTION_DEBUG
 const float TargetDirection = 90.0f;        // 真西に飛ぶ
 #endif
@@ -190,24 +189,32 @@
     i2c.frequency(400000);                  // I2Cの通信速度を400kHzに設定
     
     //Config読み取り
+    xbee.printf("load...");
     while(!LoadConfig()) {
         wait(0.1f);
+        xbee.printf(".");
     }
+    xbee.printf("complete\r\n");
     xbee.printf("target(%.5f, %.5f)\r\n", target_x, target_y);
     
     // SDカード初期化
+    xbee.printf("SD Init...");
     while(!SD_Init()) {
         wait(0.1f);
+        xbee.printf(".");
     }
+    xbee.printf("complete\r\n");
     
     // センサー関連の初期化
+    xbee.printf("Sensors Init...");
     SensorsInit();
+    xbee.printf("complete\r\n");
 
     //カルマンフィルタ初期化
     KalmanInit();
 
+    NVIC_SetPriority(TIM5_IRQn, 5);
     ticker.attach(&DataUpdate, dt);         // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
-    NVIC_SetPriority(TIM5_IRQn, 5);
     
     servoL.period(0.020f);                  // サーボの信号の周期は20ms
     servoR.period(0.020f);
@@ -255,7 +262,7 @@
         servoR.pulsewidth((ServoMax - ServoMin) * pull_R / (float)PullMax + ServoMin);
         
         // データをmicroSDに保存し、XBeeでPCへ送信する
-        sprintf(data, "%d, %.1f,%.1f,%.1f, %.3f,%.5f,%.5f, %.3f,%.3f,%.1f, %d, %d,%d\r\n",
+        sprintf(data, "%d, %.1f,%.1f,%.1f, %.3f,%.6f,%.6f, %.3f,%.3f,%.1f, %d, %d,%d\r\n",
                 UTC_t, yaw, pitch, roll,
                 press, gms.longitude, gms.latitude,
                 vrt_acc, height, Distance(target_p, p), 
@@ -506,6 +513,8 @@
         wait(0.1f);
     }
     p0 /= 10.0f;
+    
+    xbee.printf(".");
 }
 
 /** マイクロSDカードの初期化を行う関数