Diff: main.cpp

Revision:

23:79cdc1432160

Parent:

22:3caa2983bf1d

Child:

24:8838be99cec3

--- a/main.cpp	Tue Jun 23 15:23:38 2015 +0000
+++ b/main.cpp	Wed Jun 24 16:16:14 2015 +0000
@@ -3,7 +3,6 @@
 #include "HMC5883L.h"
 #include "LPS25H.h"
 #include "GMS6_CR6.h"
-#include "ErrorLogger.h"
 #include "Vector.h"
 #include "Matrix.h"
 #include "Vector_Matrix_operator.h"
@@ -51,8 +50,8 @@
 ConfigFile      cfg;                                // ConfigFile
 PwmOut          servoL(PB_6), servoR(PC_7);         // サーボ用PWM出力
 AnalogIn        optSensor(PC_0);                    // 照度センサ用アナログ入力
-AnalogIn        servoVcc(PA_0);                     // バッテリー電圧監視用アナログ入力(サーボ用)
-AnalogIn        logicVcc(PA_1);                     // バッテリー電圧監視用アナログ入力(ロジック用)
+AnalogIn        servoVcc(PA_1);                     // バッテリー電圧監視用アナログ入力(サーボ用)
+AnalogIn        logicVcc(PA_0);                     // バッテリー電圧監視用アナログ入力(ロジック用)
 DigitalIn       paraSensor(PB_0);                   // パラフォイルに繋がる（予定）の物理スイッチ
 Ticker          ticker;                             // 割り込みタイマー
 Timer           timer;                              // 時間計測用タイマー
@@ -64,11 +63,13 @@
 Vector  raw_gyro(3);            // 角速度(deg/s)  生
 Vector  raw_geomag(3);          // 地磁気(?)  生
 float   raw_press;              // 気圧(hPa)  生
+float   raw_temp;               // 温度(℃)　生
 /* メインループ内ではこちらを参照する */
 Vector  acc(3);                 // 加速度(m/s^2)
 Vector  gyro(3);                // 角速度(deg/s)
 Vector  geomag(3);              // 地磁気(?)
 float   press;                  // 気圧(hPa)
+float   temp;                   // 温度(℃)
 
 Vector  raw_g(3);               // 重力ベクトル  生
 Vector  g(3);                   // 重力ベクトル
@@ -107,7 +108,7 @@
 * target_x=111.222
 * target_y=33.444
 */
-float target_x = 0.0f, target_y = 0.0f;
+float target_x, target_y;
 
 /* ---------- Kalman Filter ---------- */
 // 地磁気ベクトル用
@@ -138,7 +139,7 @@
 
 /****************************** private functions ******************************/
 bool    SD_Init();                          // SDカード初期化
-void    VectorsInit();                      // 各種ベクトルの初期化
+void    SensorsInit();                      // 各種センサーの初期化
 void    KalmanInit();                       // カルマンフィルタ初期化
 bool    LoadConfig();                       // config読み取り
 int     Find_last();                        // SDカード初期化用関数
@@ -162,10 +163,9 @@
 {
 
     i2c.frequency(400000);                  // I2Cの通信速度を400kHzに設定
-
-    if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!");        // mpu6050初期化
-    if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!");       // hmc5883l初期化
-
+    
+    // センサー関連の初期化
+    SensorsInit();
 
     //Config読み取り
     while(!LoadConfig());
@@ -177,9 +177,6 @@
     //カルマンフィルタ初期化
     KalmanInit();
 
-    // 各種ベクトルの初期化
-    VectorsInit();
-
     ticker.attach(&DataUpdate, dt);         // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
     NVIC_SetPriority(TIM5_IRQn, 5);
     
@@ -217,9 +214,9 @@
         sprintf(data, "%d, %.1f,%.1f,%.1f, %.3f,%.5f,%.5f, %.3f,%.3f,%d, %.3f,%d, %d,%d\r\n",
                 UTC_t, yaw, pitch, roll,
                 press, gms.longitude, gms.latitude,
-                lv, vrt_acc, loopTime, 
-                Distance(target_p, p), optSensor.read_u16()
-                , pull_R, pull_L);
+                vrt_acc, temp, (int)paraSensor, 
+                Distance(target_p, p), optSensor.read_u16(), 
+                pull_R, pull_L);
         
         INT_flag = TRUE;            // 割り込み許可
         
@@ -329,7 +326,7 @@
                 /* 北から西回りで目標方向の角度を出力 */
                 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 );
+                float theta = -atan2f( targetY, targetX ) * RAD_TO_DEG;
                 float delta_theta = 0.0f;
 
                 if(yaw >= 0.0f) {                                       // ヨー角が正
@@ -402,11 +399,15 @@
     }
 }
 
-/** 各種ベクトルの初期化を行う関数
+/** 各種センサーの初期化を行う関数
  *
  */
-void VectorsInit()
+void SensorsInit()
 {
+    
+    if(!mpu.init()) error("mpu6050 Initialize Error !!");        // mpu6050初期化
+    if(!hmc.init()) error("hmc5883l Initialize Error !!");       // hmc5883l初期化
+    
     //重力ベクトルの初期化
     raw_g.SetComp(1, 0.0f);
     raw_g.SetComp(2, 0.0f);
@@ -435,7 +436,7 @@
     char filename[15];
     int n = Find_last();
     if(n < 0) {
-        pc.printf("Could not read a SD Card.\n");
+        xbee.printf("Could not read a SD Card.\n");
         return false;
     }
     sprintf(filename, "/sd/log%03d.csv", n+1);
@@ -454,18 +455,18 @@
     char value[20];
     //Read a configuration file from a mbed.
     if (!cfg.read("/sd/config.txt")) {
-        pc.printf("Config file does not exist\n");
+        xbee.printf("Config file does not exist\n");
         return false;
     } else {
         //Get values
         if (cfg.getValue("target_x", &value[0], sizeof(value))) target_x = atof(value);
         else {
-            pc.printf("Failed to get value for target_x\n");
+            xbee.printf("Failed to get value for target_x\n");
             return false;
         }
         if (cfg.getValue("target_y", &value[0], sizeof(value))) target_y = atof(value);
         else {
-            pc.printf("Failed to get value for target_y\n");
+            xbee.printf("Failed to get value for target_y\n");
             return false;
         }
     }
@@ -684,6 +685,8 @@
 
 }
 
+/* ------------------------------ 割り込み関数 ------------------------------ */
+
 /** データ取得＆更新関数
  *
  */
@@ -709,6 +712,7 @@
     lps_cnt = (lps_cnt+1)%10;
     if(lps_cnt == 0) {
         raw_press = (float)lps.pressure() * PRES_LSB_TO_HPA;
+        //raw_temp = TempLsbToDeg(lps.temperature());
     }
 
     if(INT_flag != FALSE) {
@@ -719,14 +723,13 @@
         acc = raw_acc;
         gyro = raw_gyro;
         press = raw_press;
+        temp = raw_temp;
 
         vrt_acc = raw_acc * raw_g;
 
     }
 }
 
-/* ------------------------------ 割り込み関数 ------------------------------ */
-
 /* ------------------------------ デバッグ用関数 ------------------------------ */
 
 void toString(Matrix& m)