MPUとHMCでうごくかもver
Dependencies: ConfigFile SDFileSystem mbed
Fork of LAURUS_program by
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; // バッファが空の場合は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