Diff: main.cpp

Revision:

7:0ec343d29641

Parent:

6:2b68f85a984a

Child:

10:8ee11e412ad7

--- a/main.cpp	Fri Jun 12 04:00:23 2015 +0000
+++ b/main.cpp	Mon Jun 15 00:50:28 2015 +0000
@@ -8,7 +8,9 @@
 #include "Matrix.h"
 #include "Vector_Matrix_operator.h"
 #include "myConstants.h"
-#include "Log.h"
+#include "SDFileSystem.h"
+#include "BufferedSerial.h"
+#include "ConfigFile.h"
 
 /********** private define    **********/
 #define TRUE    1
@@ -18,20 +20,22 @@
 /********** private typedef   **********/
 
 /********** private variables **********/
-DigitalOut myled(LED1);                 // デバッグ用LEDのためのデジタル出力
-I2C         i2c(PB_9, PB_8);            // I2Cポート
-MPU6050     mpu(&i2c);                  // 加速度・角速度センサ
-HMC5883L    hmc(&i2c);                  // 地磁気センサ
-LPS25H      lps(&i2c);                  // 気圧センサ
-Serial      gps(PA_11, PA_12);                // GPS通信用シリアルポート
-Serial      pc(SERIAL_TX, SERIAL_RX);   // PC通信用シリアルポート
-GMS6_CR6    gms(&gps, &pc);             // GPS
-Ticker      INT_timer;                      // 割り込みタイマー
-Log         logger(PB_5, PB_4, PB_3, PB_10, PA_9, PA_10, PC_1);    // ロガー（microSD、XBee）
-PwmOut      servoL(PC_7), servoR(PB_6);
-AnalogIn    rf(PC_0);
-AnalogIn    servoVcc(PA_0);
-AnalogIn    logicVcc(PA_1);
+DigitalOut      myled(LED1);                 // デバッグ用LEDのためのデジタル出力
+I2C             i2c(PB_9, PB_8);            // I2Cポート
+MPU6050         mpu(&i2c);                  // 加速度・角速度センサ
+HMC5883L        hmc(&i2c);                  // 地磁気センサ
+LPS25H          lps(&i2c);                  // 気圧センサ
+Serial          gps(PA_11, PA_12);                // GPS通信用シリアルポート
+Serial          pc(SERIAL_TX, SERIAL_RX);   // PC通信用シリアルポート
+GMS6_CR6        gms(&gps, &pc);             // GPS
+Ticker          INT_timer;                      // 割り込みタイマー
+SDFileSystem    sd(PB_5, PB_4, PB_3, PB_10, "sd");  // microSD
+BufferedSerial  xbee(PA_9, PA_10, PC_1);            // Xbee
+ConfigFile      cfg;                                //ConfigFile
+PwmOut          servoL(PC_7), servoR(PB_6);
+AnalogIn        rf(PC_0);
+AnalogIn        servoVcc(PA_0);
+AnalogIn        logicVcc(PA_1);
 
 
 const float dt = 0.1f;           // 割り込み周期（s）
@@ -51,6 +55,14 @@
 Vector g(3);                        // 重力ベクトル  生
 //Vector n(3);                        // 地磁気ベクトル
 
+/** config.txt **
+* #から始めるのはコメント行 
+* #イコールの前後に空白を入れない
+* target_x=111.222
+* target_y=33.444
+*/
+float target_x, target_y;
+
 /* ----- Kalman Filter ----- */
 Vector pri_x(6);
 Matrix pri_P(6, 6);
@@ -68,6 +80,8 @@
 char data[512] = {};
 
 /********** private functions **********/
+void LoadConfig();                  // config読み取り
+int find_last();                    // SDカード初期化用関数
 void KalmanInit();                  // カルマンフィルタ初期化
 void KalmanUpdate();                // カルマンフィルタ更新
 void INT_func();                    // 割り込み用関数
@@ -83,9 +97,23 @@
     if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!");        // mpu6050初期化
     if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!");       // hmc5883l初期化
     
-    char* title = "log data\r\n";                                       // ログのタイトル行
-    logger.initialize_sdlog(title);                                     // ログファイル初期設定
+    //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");
+    
+    //カルマンフィルタ初期化
     KalmanInit();
     
     INT_timer.attach(&INT_func, dt);  // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
@@ -115,7 +143,8 @@
                 geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3), 
                 press, gms.longitude, gms.latitude, 
                 sv, lv, gms.Ns);
-        logger.write(data);
+        fprintf(fp, data);
+        xbee.printf(data);
         
         INT_flag = TRUE;            // 割り込み許可
         
@@ -132,6 +161,44 @@
     }
     
     /* ---------- ↑↑↑ ここまでメインループ ↑↑↑ ---------- */
+    //fclose(fp);
+}
+
+void LoadConfig(){
+    char value[20];
+    //Read a configuration file from a mbed.
+    if (!cfg.read("/sd/config.txt")){
+        pc.printf("Config file does not exist\n");
+    }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");
+        }
+        if (cfg.getValue("target_y", &value[0], sizeof(value))) target_y = atof(value);
+        else{
+            pc.printf("Failed to get value for target_y\n");
+        }
+    }
+}
+
+int find_last() {
+    int i, n = 0;
+    char c;
+    DIR *dp;
+    struct dirent *dirst;
+    dp = opendir("/sd/");
+    if (!dp){
+        pc.printf("Could not open directry.\n");
+        return -1;
+    }
+    while((dirst = readdir(dp)) != NULL) {
+        if(sscanf(dirst->d_name, "log%03d.csv%c", &i, &c) == 1 && i>n) {
+            n = i;
+        }
+    }
+    closedir(dp);
+    return n;
 }
 
 void KalmanInit() {