MPU6050のサンプルプログラム2
Dependencies: ConfigFile SDFileSystem mbed
Fork of LAURUS_program by
Revision 1:6cd6d2760856, committed 2015-05-24
- Comitter:
- ojan
- Date:
- Sun May 24 17:32:47 2015 +0000
- Parent:
- 0:bc6f14fc60c7
- Child:
- 2:d2b60a1d0cd9
- Commit message:
- Send all sensor's data to PC
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/GMS6_CR6/GMS6_CR6.cpp Sun May 24 17:32:47 2015 +0000 @@ -0,0 +1,49 @@ +#include "mbed.h" +#include "GMS6_CR6.h" + +GMS6_CR6::GMS6_CR6(Serial* ps, Serial* pc): p_port(ps), p_pc(pc) { + p_port->baud(4800); + pointer = 0; + INT_flag = 0; + p_port->attach(this, &GMS6_CR6::INT_Rx, Serial::RxIrq); +} + +GMS6_CR6::~GMS6_CR6() { + p_port = NULL; +} + +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); + if(!ret) { + p_pc->printf("sscanf Error\r\n"); + return; + } + + int deg_lat = (int)raw_latitude / 100; + float min_lat = (raw_latitude - (float)(deg_lat*100)) / 60.0f; + latitude = (float)deg_lat + min_lat; + + int deg_lng = (int)raw_longitude / 100; + float min_lng = (raw_longitude - (float)(deg_lng*100)) / 60.0f; + longitude = (float)deg_lng + min_lng; + + +} + +void GMS6_CR6::INT_Rx() { + buff1[pointer] = p_port->getc(); + + if(buff1[pointer] != '\r') { + pointer = (pointer+1)%BuffSize; + } else { + if(strstr((const char*)buff1, "GPGGA") != NULL) { + buff1[pointer] = '\0'; + INT_flag = 1; + strcpy(buff2, (const char*)buff1); + INT_flag = 0; + pointer = 0; + } + } +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/GMS6_CR6/GMS6_CR6.h Sun May 24 17:32:47 2015 +0000 @@ -0,0 +1,25 @@ +#pragma once + +const int BuffSize = 256; + +class GMS6_CR6 { + +public: + GMS6_CR6(Serial* ps, Serial* pc); + ~GMS6_CR6(); + void INT_Rx(); + void read(); + + char lat_hem, lng_hem; + float raw_longitude, raw_latitude; + float longitude, latitude; + float time; + +private: + Serial* p_port; + Serial* p_pc; + char buff1[BuffSize]; + char buff2[BuffSize]; + int pointer; + volatile int INT_flag; +}; \ No newline at end of file
--- a/HMC5883L/HMC5883L.cpp Fri May 15 17:24:32 2015 +0000 +++ b/HMC5883L/HMC5883L.cpp Sun May 24 17:32:47 2015 +0000 @@ -1,9 +1,7 @@ #include "mbed.h" #include "HMC5883L.h" -HMC5883L::HMC5883L(I2C* i2c) { - this->i2c = i2c; -} +HMC5883L::HMC5883L(I2C* p_i2c): i2c(p_i2c) {} HMC5883L::~HMC5883L() { i2c = NULL; @@ -30,9 +28,15 @@ data.reg[i*2+1] = temp; } + // 軸を加速度センサーとあわせる int16_t temp = data.value[1]; data.value[1] = data.value[2]; data.value[2] = temp; + temp = data.value[1]; + data.value[1] = -data.value[0]; + data.value[0] = temp; + + return 1; } \ No newline at end of file
--- a/HMC5883L/HMC5883L.h Fri May 15 17:24:32 2015 +0000 +++ b/HMC5883L/HMC5883L.h Sun May 24 17:32:47 2015 +0000 @@ -19,7 +19,7 @@ class HMC5883L { public: - HMC5883L(I2C* i2c); + HMC5883L(I2C* p_i2c); ~HMC5883L(); int init();
--- a/MPU6050/MPU6050.cpp Fri May 15 17:24:32 2015 +0000 +++ b/MPU6050/MPU6050.cpp Sun May 24 17:32:47 2015 +0000 @@ -1,8 +1,7 @@ #include "mbed.h" #include "MPU6050.h" -MPU6050::MPU6050(I2C* i2c) { - this->i2c = i2c; +MPU6050::MPU6050(I2C* p_i2c): i2c(p_i2c){ } MPU6050::~MPU6050() { @@ -10,10 +9,21 @@ } int MPU6050::init() { + // スリープモード解除 char cmd[2] = {0x6b, 0x00}; int ret = i2c->write(mpu_addr, cmd, 2); + if(ret != 0) return 0; + // ジャイロのレンジを500deg/sに設定 + char data = 0; + cmd[0] = 0x1b; + ret = i2c->write(mpu_addr, cmd, 1, true); if(ret != 0) return 0; + i2c->read(mpu_addr | 0x01, &data, 1, false); + cmd[1] = data | 0x08; + ret = i2c->write(mpu_addr, cmd, 2); + if(ret != 0) return 0; + return 1; }
--- a/MPU6050/MPU6050.h Fri May 15 17:24:32 2015 +0000 +++ b/MPU6050/MPU6050.h Sun May 24 17:32:47 2015 +0000 @@ -34,7 +34,7 @@ class MPU6050 { public: - MPU6050(I2C* i2c); + MPU6050(I2C* p_i2c); ~MPU6050(); int init();
--- a/main.cpp Fri May 15 17:24:32 2015 +0000 +++ b/main.cpp Sun May 24 17:32:47 2015 +0000 @@ -2,6 +2,7 @@ #include "MPU6050.h" #include "HMC5883L.h" #include "LPS25H.h" +#include "GMS6_CR6.h" #include "ErrorLogger.h" #include "Vector.h" #include "myConstants.h" @@ -10,25 +11,35 @@ #define TRUE 1 #define FALSE 0 /********** private macro **********/ + /********** private typedef **********/ + /********** private variables **********/ -I2C i2c(I2C_SDA, I2C_SCL); // I2Cポート -MPU6050 mpu(&i2c); // 加速度・角速度センサ -HMC5883L hmc(&i2c); // 地磁気センサ -LPS25H lps(&i2c); // 気圧センサ -Serial pc(SERIAL_TX, SERIAL_RX); // PC通信用シリアルポート -Ticker timer; // 割り込みタイマー +I2C i2c(I2C_SDA, I2C_SCL); // I2Cポート +MPU6050 mpu(&i2c); // 加速度・角速度センサ +HMC5883L hmc(&i2c); // 地磁気センサ +LPS25H lps(&i2c); // 気圧センサ +Serial gps(D8, D2); // GPS通信用シリアルポート +Serial pc(SERIAL_TX, SERIAL_RX); // PC通信用シリアルポート +GMS6_CR6 gms(&gps, &pc); // GPS +Ticker timer; // 割り込みタイマー -const float freq = 0.01f; // 割り込み周期(s) +const float Freq = 0.01f; // 割り込み周期(s) int lps_cnt = 0; // 気圧センサ読み取りカウント -uint8_t INT_flag = TRUE; // 割り込み可否フラグ -float acc[3]; // 加速度(m/s^2) -float gyro[3]; // 角速度(deg/s) -float geomag[3]; // 地磁気(?) +uint8_t INT_flag = FALSE; // 割り込み可否フラグ +Vector acc(3); // 加速度(m/s^2) +Vector gyro(3); // 角速度(deg/s) +Vector geomag(3); // 地磁気(?) float press; // 気圧(hPa) + +Vector g(3); // 重力ベクトル +Vector n(3); // 地磁気ベクトル +Vector bias(3); // 地磁気センサのバイアスベクトル + /********** private functions **********/ void INT_func(); // 割り込み用関数 + /********** main function **********/ int main() { @@ -38,23 +49,31 @@ if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!"); // mpu6050初期化 if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!"); // hmc5883l初期化 - timer.attach(&INT_func, freq); // 割り込み有効化 + timer.attach(&INT_func, Freq); // 割り込み有効化(Freq = 0.01fなので、10msおきの割り込み) + + //重力ベクトルの初期化 + g.SetComp(1, 0.0f); + g.SetComp(2, 0.0f); + g.SetComp(3, 1.0f); while(1) { + // 1秒おきにセンサーの出力をpcへ出力 + wait(1.0f); + INT_flag = FALSE; // 割り込みによる変数書き換えを阻止 // センサ類の全出力値をPCに送信 - printf("%f\t", acc[0]); - printf("%f\t", acc[1]); - printf("%f\t", acc[2]); - printf("%f\t", gyro[0]); - printf("%f\t", gyro[1]); - printf("%f\t", gyro[2]); - printf("%f\t", geomag[0]); - printf("%f\t", geomag[1]); - printf("%f\t", geomag[2]); - printf("%f\r\n", press); + pc.printf("%f,", acc.GetComp(1)); + pc.printf("%f,", acc.GetComp(2)); + pc.printf("%f,", acc.GetComp(3)); + pc.printf("%f,", gyro.GetComp(1)); + pc.printf("%f,", gyro.GetComp(2)); + pc.printf("%f,", gyro.GetComp(3)); + pc.printf("%f,", geomag.GetComp(1)); + pc.printf("%f,", geomag.GetComp(2)); + pc.printf("%f,", geomag.GetComp(3)); + pc.printf("%f\r\n", press); INT_flag = TRUE; // 割り込み許可 @@ -64,15 +83,24 @@ void INT_func() { if(INT_flag == FALSE) { + // センサーの値を更新 mpu.read(); hmc.read(); for(int i=0; i<3; i++) { - acc[i] = (float)mpu.data.value.acc[i] * ACC_LSB_TO_G * G_TO_MPSS; - gyro[i] = mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG; - geomag[i] = hmc.data.value[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(gyro, g); // Δg = ω × g + g = 0.9f * (g - delta_g * Freq) + 0.1f * acc.Normalize(); // 相補フィルタ + g = g.Normalize(); + + Vector delta_n = Cross(gyro,n); + n = 0.9f * (n - delta_n * Freq) + 0.1f * geomag.Normalize(); + n = n.Normalize(); + // LPS25Hによる気圧の取得は10Hz lps_cnt = (lps_cnt+1)%10; if(lps_cnt == 0) {
--- a/myConstants.h Fri May 15 17:24:32 2015 +0000 +++ b/myConstants.h Sun May 24 17:32:47 2015 +0000 @@ -6,6 +6,11 @@ #define DEG_TO_RAD 0.0174532925f // π / 180 #define ACC_LSB_TO_G 0.0000610351562f // g/LSB (1/2^14 #define G_TO_MPSS 9.8f // (m/s^2)/g -//#define GYRO_LSB_TO_DEG 0.0152671755f // deg/LSB (1/65.5 -#define GYRO_LSB_TO_DEG 0.00763358778f // deg/LSB (1/131 -#define PRES_LSB_TO_HPA 0.000244140625f // hPa/LSB (1/4096 \ No newline at end of file +//#define GYRO_LSB_TO_DEG 0.0304878048f // deg/LSB (1/32.8 +#define GYRO_LSB_TO_DEG 0.0152671755f // deg/LSB (1/65.5 +//#define GYRO_LSB_TO_DEG 0.00763358778f // deg/LSB (1/131 +#define PRES_LSB_TO_HPA 0.000244140625f // hPa/LSB (1/4096 + +#define MAG_LSB_TO_GAUSS 0.00092f // Gauss/LSB +#define MAG_MAGNITUDE 0.46f // Magnitude of GeoMagnetism (Gauss) +#define MAG_SIN -0.754709580f // Sin-Value of Inclination \ No newline at end of file