Revision 4:45dc5590abc0, committed 2015-06-11

Comitter:

ojan

Date:

Thu Jun 11 15:43:07 2015 +0000

Parent:

3:5358a691a100

Child:

5:182f6356bce1

Commit message:

add flow control

Changed in this revision

--- a/ErrorLogger.cpp	Sat May 30 18:08:34 2015 +0000
+++ b/ErrorLogger.cpp	Thu Jun 11 15:43:07 2015 +0000
@@ -6,6 +6,7 @@
 }
 
 void AbortWithMsg(const char* msg) {
+    while(!debugPort->writeable());
     debugPort->printf(msg);
     abort();
 }
\ No newline at end of file

--- a/GMS6_CR6/GMS6_CR6.cpp	Sat May 30 18:08:34 2015 +0000
+++ b/GMS6_CR6/GMS6_CR6.cpp	Thu Jun 11 15:43:07 2015 +0000
@@ -15,7 +15,7 @@
 void GMS6_CR6::read() {
     while(INT_flag);
     int ret = sscanf(buff2, "$GPGGA,%f,%f,%c,%f,%c,%d,%d", 
-                &time, &raw_latitude, &lat_hem, &raw_longitude, &lng_hem);
+                &time, &raw_latitude, &lat_hem, &raw_longitude, &lng_hem, &mode, &Ns);
     if(!ret) {
         p_pc->printf("sscanf Error\r\n");
         return;

--- a/GMS6_CR6/GMS6_CR6.h	Sat May 30 18:08:34 2015 +0000
+++ b/GMS6_CR6/GMS6_CR6.h	Thu Jun 11 15:43:07 2015 +0000
@@ -14,6 +14,8 @@
     float raw_longitude, raw_latitude;
     float longitude, latitude;
     float time;
+    int mode;
+    int Ns;
       
 private: 
     Serial* p_port;

--- a/Log/Log.cpp	Sat May 30 18:08:34 2015 +0000
+++ b/Log/Log.cpp	Thu Jun 11 15:43:07 2015 +0000
@@ -1,9 +1,9 @@
 #include "Log.h"
 
-Log::Log(PinName rx, PinName tx, PinName mosi, PinName miso, PinName sck, PinName cs, const char* name) :
-    _device(rx, tx), _sd(mosi, miso, sck, cs, name){
+Log::Log(PinName tx, PinName rx, PinName cts, PinName mosi, PinName miso, PinName sck, PinName cs) :
+    _sd(mosi, miso, sck, cs, "sd"), _device(tx, rx), ctsPin(cts){
     buf_send.initialize_buffer();
-    _device.attach(this, &Log::int_serial_tx, Serial::TxIrq);
+    //_device.attach(this, &Log::int_serial_tx, Serial::TxIrq);
 }
 
 int Log::initialize_sdlog(const char* str){
@@ -18,6 +18,8 @@
     
     _device.printf(str);
     fprintf(fp, str);
+    fflush(fp);
+    
     return 1;
 }
 
@@ -47,7 +49,14 @@
 }
 
 void Log::puts(const char* str){
-    int16_t len=strlen(str);
+    fputs(str, fp);
+    fflush(fp);
+    //while(!_device.writeable());
+    for(int i=0; str[i] != '\0'; i++) {
+        while(ctsPin);
+        _device.putc(str[i]);
+    }
+    /*int16_t len=strlen(str);
     int16_t capa=buf_send.buffer_capacity();
     bool empty=buf_send.is_buffer_empty();
     char ch;
@@ -59,13 +68,16 @@
         ch=buf_send.read_buffer_byte();
         _device.putc(ch);
         fputc(ch, fp);
-    } 
+        fflush(fp);
+    }*/
 }
 
 void Log::putc(char ch){
+    
     int16_t capa=buf_send.buffer_capacity();
     if(capa==0) return;
     buf_send.write_buffer_byte(ch);
+    
 }
 
 void Log::write_data(uint8_t* buf, int16_t size){
@@ -105,10 +117,18 @@
 }
 
 void Log::int_serial_tx(){
-    char ch;
+    /*char ch;
     while((_device.writeable()) && (buf_send.is_buffer_empty()==false)){
         ch=buf_send.read_buffer_byte();
         _device.putc(ch);
         fputc(ch, fp);
+    }*/
+    
+    uint8_t buf[RING_BUFFER_SIZE] = {};
+    int data_num = buf_send.read_buffer_all(buf);
+    if(data_num != 0) {
+        _device.printf((const char*)buf);
+        fputs((const char*)buf, fp);
+        fflush(fp);
     }
 }
\ No newline at end of file

--- a/Log/Log.h	Sat May 30 18:08:34 2015 +0000
+++ b/Log/Log.h	Thu Jun 11 15:43:07 2015 +0000
@@ -12,7 +12,7 @@
 
 class Log{
 public:
-    Log(PinName rx, PinName tx, PinName mosi, PinName miso, PinName sck, PinName cs, const char* name);
+    Log(PinName tx, PinName rx, PinName cts, PinName mosi, PinName miso, PinName sck, PinName cs);
     int initialize_sdlog(const char* str);
     void close();
     int find_last();
@@ -30,6 +30,7 @@
     SDFileSystem _sd;
     FILE *fp;
     Serial _device;
+    DigitalIn ctsPin;
     RingBuffer buf_send;
     RingBuffer buf_recieve;
 };

--- a/RingBuffer/RingBuffer.cpp	Sat May 30 18:08:34 2015 +0000
+++ b/RingBuffer/RingBuffer.cpp	Thu Jun 11 15:43:07 2015 +0000
@@ -37,6 +37,24 @@
     }
 }
 
+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;

--- a/RingBuffer/RingBuffer.h	Sat May 30 18:08:34 2015 +0000
+++ b/RingBuffer/RingBuffer.h	Thu Jun 11 15:43:07 2015 +0000
@@ -11,7 +11,8 @@
     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); 
+    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);

--- a/main.cpp	Sat May 30 18:08:34 2015 +0000
+++ b/main.cpp	Thu Jun 11 15:43:07 2015 +0000
@@ -18,7 +18,8 @@
 /********** private typedef   **********/
 
 /********** private variables **********/
-I2C         i2c(PB_9, PB_8);      // I2Cポート
+DigitalOut myled(LED1);                 // デバッグ用LEDのためのデジタル出力
+I2C         i2c(PB_9, PB_8);            // I2Cポート
 MPU6050     mpu(&i2c);                  // 加速度・角速度センサ
 HMC5883L    hmc(&i2c);                  // 地磁気センサ
 LPS25H      lps(&i2c);                  // 気圧センサ
@@ -26,18 +27,24 @@
 Serial      pc(SERIAL_TX, SERIAL_RX);   // PC通信用シリアルポート
 GMS6_CR6    gms(&gps, &pc);             // GPS
 Ticker      INT_timer;                      // 割り込みタイマー
-//Log         logger(PA_9, PA_10, PB_5, PB_4, PB_3, PB_10, "sd");    // ロガー（microSD、XBee）
+Log         logger(PA_9, PA_10, PC_1, PB_5, PB_4, PB_3, PB_10);    // ロガー（microSD、XBee）
+DigitalIn   cts(PC_1);
 
 const float dt = 0.1f;           // 割り込み周期（s）
 
 int lps_cnt = 0;                    // 気圧センサ読み取りカウント
 uint8_t INT_flag = TRUE;            // 割り込み可否フラグ
+Vector raw_acc(3);                       // 加速度(m/s^2)  生
+Vector raw_gyro(3);                      // 角速度(deg/s)  生
+Vector raw_geomag(3);                    // 地磁気(?)  生
+float raw_press;                        // 気圧(hPa)  生
 Vector acc(3);                       // 加速度(m/s^2)
 Vector gyro(3);                      // 角速度(deg/s)
 Vector geomag(3);                    // 地磁気(?)
 float press;                        // 気圧(hPa)
 
-Vector g(3);                        // 重力ベクトル
+Vector raw_g(3);                        // 重力ベクトル  生
+Vector g(3);                        // 重力ベクトル  生
 //Vector n(3);                        // 地磁気ベクトル
 
 /* ----- Kalman Filter ----- */
@@ -57,8 +64,8 @@
 char data[1024] = {};
 
 /********** private functions **********/
-void KalmanInit();
-void KalmanUpdate();
+void KalmanInit();                  // カルマンフィルタ初期化
+void KalmanUpdate();                // カルマンフィルタ更新
 void INT_func();                    // 割り込み用関数
 void toString(Matrix& m);
 void toString(Vector& v);
@@ -72,57 +79,44 @@
     if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!");        // mpu6050初期化
     if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!");       // hmc5883l初期化
     
-    //char* title = "log data\r\n";                                       // ログのタイトル行
-    //if(!logger.initialize_sdlog(title)) return 0;                       // ログファイル初期設定
+    char* title = "log data\r\n";                                       // ログのタイトル行
+    if(!logger.initialize_sdlog(title)) return 0;                       // ログファイル初期設定
     
     KalmanInit();
     
     INT_timer.attach(&INT_func, dt);  // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
     
     //重力ベクトルの初期化
-    g.SetComp(1, 0.0f);
-    g.SetComp(2, 0.0f);
-    g.SetComp(3, 1.0f);
+    raw_g.SetComp(1, 0.0f);
+    raw_g.SetComp(2, 0.0f);
+    raw_g.SetComp(3, 1.0f);
     
     /* ---------- ↓↓↓ ここからメインループ ↓↓↓ ---------- */
     while(1) {
-        
+        timer.stop();
+        timer.reset();
+        timer.start();
         // 0.1秒おきにセンサーの出力をpcへ出力
-        wait(0.1f);
+        myled = 1; // LED is ON
+        wait(0.05f); // 50 ms
+        myled = 0; // LED is OFF
         
         INT_flag = FALSE;           // 割り込みによる変数書き換えを阻止
         
-        // センサ類の全出力値をPCに送信
-        /*
-        pc.printf("%.3f,", acc.GetComp(1));
-        pc.printf("%.3f,", acc.GetComp(2));
-        pc.printf("%.3f,", acc.GetComp(3));
-        pc.printf("%.3f,", gyro.GetComp(1));
-        pc.printf("%.3f,", gyro.GetComp(2));
-        pc.printf("%.3f,", gyro.GetComp(3));
-        pc.printf("%.3f,", geomag.GetComp(1));
-        pc.printf("%.3f,", geomag.GetComp(2));
-        pc.printf("%.3f\r\n", geomag.GetComp(3));
-        */
-        //pc.printf("%.3f\r\n", press);
-        
-        
-        pc.printf("%.3f,", geomag.GetComp(1));
-        pc.printf("%.3f,", geomag.GetComp(2));
-        pc.printf("%.3f,", geomag.GetComp(3));
-        pc.printf("%.3f,", post_x.GetComp(1));
-        pc.printf("%.3f,", post_x.GetComp(2));
-        pc.printf("%.3f\r\n", post_x.GetComp(3));
-        
-        /*
-        sprintf(data, "%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", 
+        sprintf(data, "%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", 
                 acc.GetComp(1), acc.GetComp(2), acc.GetComp(3), 
                 gyro.GetComp(1), gyro.GetComp(2), gyro.GetComp(3), 
-                geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3), press);
+                geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3), 
+                press, gms.longitude, gms.latitude);
         logger.puts(data);
-        */
+        
         INT_flag = TRUE;            // 割り込み許可
         
+        pc.printf("time: %.3f[s]\r\n", (float)timer.read_ms()/1000.0f);
+        
+        // ループはきっかり1秒ごと
+        while(timer.read_ms() < 1000);
+        
     }
     
     /* ---------- ↑↑↑ ここまでメインループ ↑↑↑ ---------- */
@@ -138,9 +132,9 @@
     
     // 状態方程式のヤコビアンの初期値を代入（時間変化あり）
     float f[36] = {
-        1.0f, gyro.GetComp(3)*dt, -gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f, 
-        -gyro.GetComp(3)*dt, 1.0f, gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f, 
-        gyro.GetComp(2)*dt, -gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f, 
+        1.0f, raw_gyro.GetComp(3)*dt, -raw_gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f, 
+        -raw_gyro.GetComp(3)*dt, 1.0f, raw_gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f, 
+        raw_gyro.GetComp(2)*dt, -raw_gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f, 
         0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 
         0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,  
         0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
@@ -161,9 +155,9 @@
 void KalmanUpdate() {
     // ヤコビアンの更新
     float f[36] = {
-        1.0f, gyro.GetComp(3)*dt, -gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f, 
-        -gyro.GetComp(3)*dt, 1.0f, gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f, 
-        gyro.GetComp(2)*dt, -gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f, 
+        1.0f, raw_gyro.GetComp(3)*dt, -raw_gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f, 
+        -raw_gyro.GetComp(3)*dt, 1.0f, raw_gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f, 
+        raw_gyro.GetComp(2)*dt, -raw_gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f, 
         0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 
         0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,  
         0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
@@ -186,42 +180,47 @@
     K = pri_P * H.Transpose() * inv;    
     
     // 事後推定値の更新
-    post_x = pri_x + K * (geomag - H * pri_x);
+    post_x = pri_x + K * (raw_geomag - H * pri_x);
     // 事後誤差分散行列の更新
     post_P = (I - K * H) * pri_P;
 }
 
 void INT_func() {
-    if(INT_flag != FALSE) {
+    // センサーの値を更新
+    mpu.read();
+    hmc.read();
+    
+    for(int i=0; i<3; i++) {
+        raw_acc.SetComp(i+1, (float)mpu.data.value.acc[i] * ACC_LSB_TO_G);
+        raw_gyro.SetComp(i+1, (float)mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD);
+        raw_geomag.SetComp(i+1, (float)hmc.data.value[i] * MAG_LSB_TO_GAUSS);
+    }
         
-        timer.reset();
-        timer.start();
-        // センサーの値を更新
-        mpu.read();
-        hmc.read();
-        
-        for(int i=0; i<3; i++) {
-            acc.SetComp(i+1, (float)mpu.data.value.acc[i] * ACC_LSB_TO_G);
-            gyro.SetComp(i+1, (float)mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD);
-            geomag.SetComp(i+1, (float)hmc.data.value[i] * MAG_LSB_TO_GAUSS);
-        }
+    Vector delta_g = Cross(raw_gyro, raw_g);                            // Δg = ω × g
+    raw_g = 0.9f * (raw_g - delta_g * dt) + 0.1f * raw_acc.Normalize();   // 相補フィルタ
+    raw_g = raw_g.Normalize();
+       
+    KalmanUpdate();
         
-        Vector delta_g = Cross(gyro, g);                            // Δg = ω × g
-        g = 0.9f * (g - delta_g * dt) + 0.1f * acc.Normalize();   // 相補フィルタ
-        g = g.Normalize();
-        
-        
-        KalmanUpdate();
-        
+    // LPS25Hによる気圧の取得は10Hz
+    lps_cnt = (lps_cnt+1)%10;
+    if(lps_cnt == 0) {
+        raw_press = (float)lps.pressure() * PRES_LSB_TO_HPA;
+    }
+    //pc.printf("%d(us)\r\n", timer.read_us());
+    
+    //pc.printf("PC_1 = %d\r\n", (int)cts);
+    //pc.printf("test\r\n");
+    
+    if(INT_flag != FALSE) {
+        g = raw_g;
+        for(int i=0; i<3; i++) {
+            geomag.SetComp(i+1, post_x.GetComp(i+1));
+        }
+        acc = raw_acc;
+        gyro = raw_gyro;
+        press = raw_press;
         
-        // LPS25Hによる気圧の取得は10Hz
-        lps_cnt = (lps_cnt+1)%10;
-        if(lps_cnt == 0) {
-            press = (float)lps.pressure() * PRES_LSB_TO_HPA;
-        }
-        
-        timer.stop();
-        //pc.printf("%d(us)\r\n", timer.read_us());
     }
 }
 

--- a/mbed.bld	Sat May 30 18:08:34 2015 +0000
+++ b/mbed.bld	Thu Jun 11 15:43:07 2015 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/8ab26030e058
\ No newline at end of file
+http://mbed.org/users/mbed_official/code/mbed/builds/cbbeb26dbd92
\ No newline at end of file