Revision 7:0ec343d29641, committed 2015-06-15

Comitter:

onaka

Date:

Mon Jun 15 00:50:28 2015 +0000

Parent:

6:2b68f85a984a

Child:

10:8ee11e412ad7

Commit message:

add config loader

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ConfigFile.lib	Mon Jun 15 00:50:28 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/shintamainjp/code/ConfigFile/#f6ceafabe9f8

--- a/Log/Log.cpp	Fri Jun 12 04:00:23 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,51 +0,0 @@
-#include "Log.h"
-
-Log::Log(PinName mosi, PinName miso, PinName sck, PinName cs, PinName tx, PinName rx, PinName cts) :
-    _sd(mosi, miso, sck, cs, "sd"), _device(tx, rx, cts){
-    
-}
-
-int Log::initialize_sdlog(const char* str){
-    char filename[15];
-    int n = find_last();
-    if(n < 0) return 0;
-    
-    //ログ番号を+1してファイルを新規作成
-    //ファイル名は"logXXX.csv"
-    sprintf(filename, "/sd/log%03d.csv", n+1);
-    fp = fopen(filename, "w");
-    
-    fprintf(fp, str);
-    _device.printf(str);
-    return 1;
-}
-
-void Log::close(){
-    wait(0.5);
-    fclose(fp);
-}
-
-//SDカード内からログ番号の最大値を取得する関数
-int Log::find_last() {
-    int i, n = 0;
-    char c;
-    DIR *dp;
-    struct dirent *dirst;
-    dp = opendir("/sd/");
-    if (!dp){
-        error("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 Log::write(const char* str){
-    fprintf(fp, str);
-    _device.printf(str);
-}
\ No newline at end of file

--- a/Log/Log.h	Fri Jun 12 04:00:23 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,27 +0,0 @@
-//**** Log.h ****
-//ログをリングバッファを用いて、
-//シリアル送信、SDカード出力を行う
-//***************
-
-#ifndef LOG_H_ 
-#define LOG_H_ 
-
-#include "mbed.h"
-#include "SDFileSystem.h"
-#include "BufferedSerial.h"
-
-class Log{
-public:
-    Log(PinName mosi, PinName miso, PinName sck, PinName cs, PinName tx, PinName rx, PinName cts);
-    int initialize_sdlog(const char* str);
-    void close();
-    int find_last();
-    void write(const char* str);
-
-protected:
-    SDFileSystem _sd;
-    BufferedSerial _device;
-    FILE *fp;
-};
-
-#endif /* LOG_H_ */
\ No newline at end of file

--- a/RingBuffer/RingBuffer.cpp	Fri Jun 12 04:00:23 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,81 +0,0 @@
-#include "RingBuffer.h"
-
-void RingBuffer::initialize_buffer(){
-    start=0;
-    end=0;
-}
-
-int16_t RingBuffer::buffer_size(){
-    if(end >= start){
-        return end - start;
-    }
-    return RING_BUFFER_SIZE - (start - end);
-}
-
-uint8_t RingBuffer::read_buffer_byte(){
-    uint8_t a;
-    a=buffer[start];
-    start=(start+1)%RING_BUFFER_SIZE;
-    return a;
-}
-
-int16_t RingBuffer::read_buffer_short(){
-    int16_t a, b;
-    a=buffer[start];
-    b=buffer[(start+1)%RING_BUFFER_SIZE];
-    start=(start+2)%RING_BUFFER_SIZE;
-    return (a&0xff)|(b<<8);
-}
-
-void RingBuffer::read_buffer(uint8_t buf[], int16_t offset, int16_t size){
-    int16_t i;
-    uint8_t* p=buf+offset;
-    
-    for(i=0; i<size; i++){
-        *(p++)=buffer[start];
-        start=(start+1)%RING_BUFFER_SIZE;
-    }
-}
-
-int16_t RingBuffer::read_buffer_all(uint8_t buf[]) {
-    if(start == end) return 0;                      // バッファが空の場合は０を返す
-    int n = 0;                                      // バッファにあるデータのバイト数
-    if(end <= start) {
-        n = RING_BUFFER_SIZE+(end-start);
-        for(int i=0; i<n; i++) {
-            buf[i] = buffer[start];
-            start = (start+1)%RING_BUFFER_SIZE;
-        }
-    } else {
-        n = end-start;
-        memcpy(buf, buffer+start, n);
-        start = end;
-    }
-    buf[n]='\0';                                //文字列の最後にヌル文字を入れる
-    return n;
-}
-
-int16_t RingBuffer::buffer_capacity(){
-    if(end >= start){
-        return RING_BUFFER_SIZE-(end-start)-1;
-    }
-    return (start-end)-1;
-}
-
-void RingBuffer::write_buffer_byte(uint8_t ch){
-    buffer[end]=ch;
-    end=(end+1)%RING_BUFFER_SIZE;
-}
-
-void RingBuffer::write_buffer(const uint8_t buf[], int16_t offset, int16_t size){
-    int16_t i;
-    const uint8_t* p=buf+offset;
-    for(i=0; i<size; i++){
-        buffer[end]=*(p++);
-        end=(end+1)%RING_BUFFER_SIZE;
-    }
-}
-
-bool RingBuffer::is_buffer_empty(){
-    return (start==end);
-}
\ No newline at end of file

--- a/RingBuffer/RingBuffer.h	Fri Jun 12 04:00:23 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,27 +0,0 @@
-#ifndef RING_BUFFER_H_
-#define RING_BUFFER_H_
-
-#include "mbed.h"
-
-#define RING_BUFFER_SIZE 512
-
-class RingBuffer{
-public:
-    void initialize_buffer();
-    int16_t buffer_size();
-    uint8_t read_buffer_byte();
-    int16_t read_buffer_short();
-    void read_buffer(uint8_t buf[], int16_t offset, int16_t size);
-    int16_t read_buffer_all(uint8_t buf[]);
-    int16_t buffer_capacity();
-    void write_buffer_byte(uint8_t ch);
-    void write_buffer(const uint8_t buf[], int16_t offset, int16_t size);
-    bool is_buffer_empty();
-
-protected:
-    uint8_t buffer[RING_BUFFER_SIZE];
-    int16_t start;
-    int16_t end;
-};
-
-#endif /* RING_BUFFER_H_ */
\ No newline at end of file

--- 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() {