MPUとHMCでうごくかもver
Dependencies: ConfigFile SDFileSystem mbed
Fork of LAURUS_program by
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() {