Luis Silvio Cordova Rivadeneira
/
mpu9250
Ejemplo de uso del MPU9250 por puerto I2C
Revision 0:cecf4940adf1, committed 2015-10-25
- Comitter:
- a0074639
- Date:
- Sun Oct 25 15:28:55 2015 +0000
- Child:
- 1:ad5417f813f4
- Commit message:
- comfirmed it works well
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sun Oct 25 15:28:55 2015 +0000 @@ -0,0 +1,52 @@ +#include "mbed.h" +#include "myMPU9250.h" + +DigitalOut g_led1(LED1), g_led2(LED2), g_led3(LED3), g_led4(LED4); +Serial g_pc(USBTX, USBRX); + +void blink_led(double sec=3.0, double interval = 0.2, + bool led1=true, bool led2=true, bool led3=true, bool led4=true) +{ + + for(double t=0.0 ; t<sec ; t+=interval*2.0) { + g_led1 = (int)led1; + g_led2 = (int)led2; + g_led3 = (int)led3; + g_led4 = (int)led4; + wait(interval); + g_led1 = g_led2 = g_led3 = g_led4 = 0; + wait(interval); + } +} + +int main() +{ + g_pc.printf("START!\n"); + MyMPU9250 imu; + if(imu.initialize()) { + g_pc.printf("SUCCESS TO INITIALIZE!\n"); + blink_led(3.0, 0.2, true, false, false, true); + } else { + blink_led(); + return 0; + } + + imu.setup(); + + double x, y, z; + for(int cnt=0;; cnt++) { + if(cnt%2==0) { + imu.updateAccelAllAxes(); + x = imu.accelX(); + y = imu.accelY(); + z = imu.accelZ(); + g_pc.printf("Accel: %f, %f, %f \n", x, y, z); + } else { + imu.updateGyroAllAxes(); + x = imu.gyroX(); + y = imu.gyroY(); + z = imu.gyroZ(); + g_pc.printf("Gyro: %f, %f, %f \n", x, y, z); + } + } +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sun Oct 25 15:28:55 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/34e6b704fe68 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mpu9250Define.h Sun Oct 25 15:28:55 2015 +0000 @@ -0,0 +1,241 @@ +#ifndef MPU9250DEFINE_H +#define MPU9250DEFINE_H + +/*-bit-*/ +#define BIT_1 (1) +#define BIT_2 (2) +#define BIT_3 (3) +#define BIT_4 (4) +#define BIT_5 (5) +#define BIT_6 (6) + +/*-send command-*/ +#define CLEAR_COMMAND (0x00) + +#define X_AXIS (1) +#define Y_AXIS (2) +#define Z_AXIS (5) +#define ALL_AXIS (X_AXIS+Y_AXIS+Z_AXIS) + +/*-MPU-9150 address-*/ +#define MPU9250_ADDRESS (0x68<<1) // 0b1101000 AD0=L + +/*-MPU-9150 register-*/ //| Bit7 | Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0 | +#define SELF_TEST_X_GYRO (0x00) +#define SELF_TEST_Y_GYRO (0x01) +#define SELF_TEST_Z_GYRO (0x02) +#define SELF_TEST_X_ACCEL (0x0D) // R/W //| XA_TEST[4-2] | XG_TEST[4-0] | +#define SELF_TEST_Y_ACCEL (0x0E) // R/W //| YA_TEST[4-2] | YG_TEST[4-0] | +#define SELE_TEST_Z_ACCEL (0x0F) // R/W //| ZA_TEST[4-2] | ZA_TEST[4-0] | + +#define SELF_TEST_A (0x10) // R/W //| RESERVED | XA_TEST[1-0]| YA_TEST[1-0]| ZA_TEST[1-0]| +#define XG_OFFSET_H (0x13) // User-defined trim values for gyroscope +#define XG_OFFSET_L (0x14) +#define YG_OFFSET_H (0x15) +#define YG_OFFSET_L (0x16) +#define ZG_OFFSET_H (0x17) +#define ZG_OFFSET_L (0x18) +#define SMPLRT_DIV (0x19) // R/W //| SMPLRT_DIV[7:0] | +#define CONFIG (0x1A) // R/W //| - | - | EXT_SYNC_SET[2:0] | DLPE_CFG[2:0] | +#define GYRO_CONFIG (0x1B) // R/W //| XG_ST| YG_ST| ZG_ST| FS_SEL[1:0] | - | - | - | +#define ACCEL_CONFIG (0x1C) // R/W //| XA_ST| YA_ST| ZA_ST| AFS_SEL[1:0]| - | +#define ACCEL_CONFIG2 (0x1D) +#define LP_ACCEL_ODR (0x1E) +#define WOM_THR (0x1F) + +#define MOT_DUR (0x20) // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms +#define ZMOT_THR (0x21) // Zero-motion detection threshold bits [7:0] +#define ZRMOT_DUR (0x22) // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms + +#define FIFO_EN (0x23) // R/W //| TEMP_FIFO_EN | XG_FIFO_EN | YG_FIFO_EN | ZG_FIFO_EN | ACCEL_FIFO_EN | SLV2_FIFO_EN | SLV1_FIFO_EN | SLV0_FIFO_EN | +#define I2C_MST_CTRL (0x24) // R/W //| MULT_MST_EN | WAIT_FOR_ES | SLV_3_FIFO_EN | I2C_MST_P_NSR | I2C_MST_CLK[3:0] | +#define I2C_SLV0_ADDR (0x25) // R/W //| I2C_SLV0_RW | I2C_SLV0_ADDR[6:0] | +#define I2C_SLV0_REG (0x26) // R/W //| I2C_SLV0_REG[7:0] | +#define I2C_SLV0_CTRL (0x27) // R/W //| I2C_SLV0_EN | I2C_SLV0_BYTE_SW | I2C_SLV0_REG_DIS | I2C_SLV0_GRP | I2C_SLV0_LEN[3:0] | +#define I2C_SLV1_ADDR (0x28) // R/W +#define I2C_SLV1_REG (0x29) // R/W +#define I2C_SLV1_CTRL (0x2A) // R/W +#define I2C_SLV2_ADDR (0x2B) // R/W +#define I2C_SLV2_REG (0x2C) // R/W +#define I2C_SLV2_CTRL (0x2D) // R/W +#define I2C_SLV3_ADDR (0x2E) // R/W +#define I2C_SLV3_REG (0x2F) // R/W +#define I2C_SLV3_CTRL (0x30) // R/W +#define I2C_SLV4_ADDR (0x31) // R/W +#define I2C_SLV4_REG (0x32) // R/W +#define I2C_SLV4_DO (0x33) // R/W +#define I2C_SLV4_CTRL (0x34) // R/W +#define I2C_SLV4_DI (0x35) // R + +#define I2C_MST_STATUS (0x36) // R +#define INT_PIN_CFG (0x37) // R/W // This register configures the behavior of the interrupt signals at the INT pins. +#define INT_ENBLE (0x38) // R/W +#define DMP_INT_STATUS (0x39) // Check DMP interrupt +#define INT_STATUS (0x3A) // R + +#define ACCEL_XOUT_H (0x3B) // R //ACCEL_XOUT[15:8] +#define ACCEL_XOUT_L (0x3C) // R //ACCEL_XOUT[7:0] +#define ACCEL_YOUT_H (0x3D) // R //ACCEL_YOUT[15:8] +#define ACCEL_YOUT_L (0x3E) // R //ACCEL_YOUT[7:0] +#define ACCEL_ZOUT_H (0x3F) // R //ACCEL_ZOUT[15:8] +#define ACCEL_ZOUT_L (0x40) // R //ACCEL_ZOUT[7:0] + +#define TEMP_OUT_H (0x41) // R +#define TEMP_OUT_L (0x42) // R + +#define GYRO_XOUT_H (0x43) // R //GYRO_XOUT[15:8] +#define GYRO_XOUT_L (0x44) // R //GYRO_XOUT[7:0] +#define GYRO_YOUT_H (0x45) // R //GYRO_YOUT[15:8] +#define GYRO_YOUT_L (0x46) // R //GYRO_YOUT[7:0] +#define GYRO_ZOUT_H (0x47) // R //GYRO_ZOUT[15:8] +#define GYRO_ZOUT_L (0x48) // R //GYRO_ZOUT[7:0] + +#define EXT_SENS_DATA_00 (0x49) // R +#define EXT_SENS_DATA_01 (0x4A) // R +#define EXT_SENS_DATA_02 (0x4B) // R +#define EXT_SENS_DATA_03 (0x4C) // R +#define EXT_SENS_DATA_04 (0x4D) // R +#define EXT_SENS_DATA_05 (0x4E) // R +#define EXT_SENS_DATA_06 (0x4F) // R +#define EXT_SENS_DATA_07 (0x50) // R +#define EXT_SENS_DATA_08 (0x51) // R +#define EXT_SENS_DATA_09 (0x52) // R +#define EXT_SENS_DATA_10 (0x53) // R +#define EXT_SENS_DATA_11 (0x54) // R +#define EXT_SENS_DATA_12 (0x55) // R +#define EXT_SENS_DATA_13 (0x56) // R +#define EXT_SENS_DATA_14 (0x57) // R +#define EXT_SENS_DATA_15 (0x58) // R +#define EXT_SENS_DATA_16 (0x59) // R +#define EXT_SENS_DATA_17 (0x5A) // R +#define EXT_SENS_DATA_18 (0x5B) // R +#define EXT_SENS_DATA_19 (0x5C) // R +#define EXT_SENS_DATA_20 (0x5D) // R +#define EXT_SENS_DATA_21 (0x5E) // R +#define EXT_SENS_DATA_22 (0x5F) // R +#define EXT_SENS_DATA_23 (0x60) // R + +#define I2C_SLV0_DO (0x63) // R/W +#define I2C_SLV1_DO (0x64) // R/W +#define I2C_SLV2_DO (0x65) // R/W +#define I2C_SLV3_DO (0x66) // R/W +#define I2C_MST_DELAY_CTRL (0x67) // R/W +#define SIGNAL_PATH_RESET (0x68) // R/W +#define USER_CTRL (0x6A) // R/W +#define PWR_MGMT_1 (0x6B) // R/W +#define PER_MGMT_2 (0x6C) // R/W + +#define FIFO_COUNTH (0x72) // R/W +#define FIFO_COUNTL (0x73) // R/W +#define FIFO_R_W (0x74) // R/W +#define WHO_AM_I (0x75) // R + +#define XA_OFFSET_H (0x77) +#define XA_OFFSET_L (0x78) +#define YA_OFFSET_H (0x7A) +#define YA_OFFSET_L (0x7B) +#define ZA_OFFSET_H (0x7D) +#define ZA_OFFSET_L (0x7E) + +/*-MPU-9150 setting command-*/ +#define ACCEL_CONFIG_AFS_SEL_2 (0x00 << 3) //16384 LSB/mg +#define ACCEL_CONFIG_AFS_SEL_4 (0x01 << 3) //8192 LSB/mg +#define ACCEL_CONFIG_AFS_SEL_8 (0x02 << 3) //4096 LSB/mg +#define ACCEL_CONFIG_AFS_SEL_16 (0x03 << 3) //2048 LSB/mg + +#define GYRO_CONFIG_FS_SEL_250 (0x00 << 3) //131 LSB/deg/sce +#define GYRO_CONFIG_FS_SEL_500 (0x01 << 3) //65.5 LSB/deg/sec +#define GYRO_CONFIG_FS_SEL_1000 (0x02 << 3) //32.8 LSB/deg/sec +#define GYRO_CONFIG_FS_SEL_2000 (0x03 << 3) //16.4 LSB/deg/sec + +/*-MPU-9150 clock source-*/ +#define PWR_MGMT_1_CLKSEL_0 (0x00) //Internal 8MHz oscillator +#define PWR_MGMT_1_CLKSEL_1 (0x01) //PLL with X axis gyroscope reference +#define PWR_MGMT_1_CLKSEL_2 (0x02) //PLL with Y axis gyroscope reference +#define PWR_MGMT_1_CLKSEL_3 (0x03) //PLL with Z axis gyroscope reference +#define PWR_MGMT_1_CLKSEL_4 (0x04) //PLL with external 32.768kHz reference +#define PWR_MGMT_1_CLKSEL_5 (0x05) //PLL with external 19.2MHz reference +#define PWR_MGMT_1_CLKSEL_6 (0x06) //Reserved +#define PWR_MGMT_1_CLKSEL_7 (0x07) //Stops the clock and keeps the timing generator in reset + +#define INT_PIN_DFG_I2C_BYPASS_EN (0x02) + +enum AccelSensitivity { + AFS_SEL_2=0, + AFS_SEL_4, + AFS_SEL_8, + AFS_SEL_16, +}; + +char getAccelSensitivity(AccelSensitivity mode) +{ + switch(mode) { + case AFS_SEL_2: + return ACCEL_CONFIG_AFS_SEL_2; + case AFS_SEL_4: + return ACCEL_CONFIG_AFS_SEL_4; + case AFS_SEL_8: + return ACCEL_CONFIG_AFS_SEL_8; + case AFS_SEL_16: + return ACCEL_CONFIG_AFS_SEL_16; + default: + return ACCEL_CONFIG_AFS_SEL_2; + } +}; + +double getAccelLSB2MG(AccelSensitivity mode) +{ + switch(mode) { + case AFS_SEL_2: + return (2.0/32768.0); + case AFS_SEL_4: + return (4.0/32768.0); + case AFS_SEL_8: + return (8.0/32768.0); + case AFS_SEL_16: + return (16.0/32768.0); + default: + return (2.0/32768.0); + } +}; + +enum GyroSensitivity { + FS_SEL_250=0, + FS_SEL_500, + FS_SEL_1000, + FS_SEL_2000, +}; + +char getGyroSensitivity(GyroSensitivity mode) +{ + switch(mode) { + case FS_SEL_250: + return GYRO_CONFIG_FS_SEL_250; + case FS_SEL_500: + return GYRO_CONFIG_FS_SEL_500; + case FS_SEL_1000: + return GYRO_CONFIG_FS_SEL_1000; + case FS_SEL_2000: + return GYRO_CONFIG_FS_SEL_2000; + default: + return GYRO_CONFIG_FS_SEL_250; + } +}; + +double getGyroLSB2DegPerSec(GyroSensitivity mode) +{ + switch(mode) { + case FS_SEL_250: + return (250.0/32768.0); + case FS_SEL_500: + return (500.0/32768.0); + case FS_SEL_1000: + return (1000.0/32768.0); + case FS_SEL_2000: + return (2000.0/32768.0); + default: + return (250.0/32768.0); + } +}; + +#endif /* MPU9250DEFINE_H */ \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/myMPU9250.h Sun Oct 25 15:28:55 2015 +0000 @@ -0,0 +1,151 @@ +#ifndef MYMPU9250_H +#define MYMPU9250_H + +#include "mpu9250Define.h" + +class MyMPU9250 +{ +public: + MyMPU9250() + : pc_(USBTX, USBRX), i2c_(p28, p27) { + accelLSB2MG_ = 10.0; + gyroLSB2DegPerSec_ = 0.0; + }; + + bool initialize() { + i2c_.frequency(400000); + i2c_.start(); + + wait(0.5); + + char cmd[2] = { 0x00 }; + cmd[0] = PWR_MGMT_1; // 0x6B + cmd[1] = (PWR_MGMT_1_CLKSEL_0); // 0x00 + this->write(cmd, BIT_2); + + wait(0.5); + + char i_am[1] = { 0x00 }; + if(! this->read(WHO_AM_I, i_am, BIT_1)) { + pc_.printf("Fail to read! I am %x\n", i_am[0]); + return false; + } else if(!(i_am[0] == 0x71)) { + pc_.printf("Fail to identify! I am %x\n", i_am[0]); + return false; + } + return true; + }; + + void setup(AccelSensitivity accel_mode=AFS_SEL_2, + GyroSensitivity gyro_mode=FS_SEL_250) { + char cmd[2] = { 0x00 }; + + cmd[0] = ACCEL_CONFIG; + cmd[1] = ACCEL_CONFIG_AFS_SEL_2;//(0x18); + this->write(cmd, BIT_2); + accelLSB2MG_ = getAccelLSB2MG(accel_mode); + pc_.printf("accelLSB2MG: %f, %x", accelLSB2MG_, cmd[1]); + + cmd[0] = GYRO_CONFIG; + cmd[1] = getGyroSensitivity(gyro_mode);//(0x18); + this->write(cmd, BIT_2); + gyroLSB2DegPerSec_ = getGyroLSB2DegPerSec(gyro_mode); + + cmd[0] = INT_PIN_CFG; // 0x37 + cmd[1] = (INT_PIN_DFG_I2C_BYPASS_EN); // 0x02 + this->write(cmd, BIT_2); + }; + + bool updateAccelAllAxes() { + return this->read(ACCEL_XOUT_H, accel_all_, BIT_6); + }; + + int accelXRaw() { + return (int16_t)(accel_all_[0]*0x100 + accel_all_[1]) ; + }; + + double accelX() { + return accelLSB2MG_ * accelXRaw() ; + }; + + int accelYRaw() { + return (int16_t)(accel_all_[2]*0x100 + accel_all_[3]) ; + }; + + double accelY() { + return accelLSB2MG_ * accelYRaw() ; + }; + + int accelZRaw() { + return (int16_t)(accel_all_[4]*0x100 + accel_all_[5]) ; + }; + + double accelZ() { + return accelLSB2MG_ * accelZRaw() ; + }; + + + bool updateGyroAllAxes() { + return this->read(GYRO_XOUT_H, gyro_all_, BIT_6); + }; + + int gyroXRaw() { + return (int16_t)(gyro_all_[0]*0x100 + gyro_all_[1]) ; + }; + + double gyroX() { + return accelLSB2MG_ * gyroXRaw() ; + }; + + int gyroYRaw() { + return (int16_t)(gyro_all_[2]*0x100 + gyro_all_[3]) ; + }; + + double gyroY() { + return accelLSB2MG_ * gyroYRaw() ; + }; + + int gyroZRaw() { + return (int16_t)(gyro_all_[4]*0x100 + gyro_all_[5]) ; + }; + + double gyroZ() { + return accelLSB2MG_ * gyroZRaw() ; + }; + + +private: + Serial pc_; + I2C i2c_; + float accelLSB2MG_; + float gyroLSB2DegPerSec_; + char accel_all_[6]; + char gyro_all_[6]; + + bool write(char* _cmd, int _length) { + int w_status = i2c_.write(MPU9250_ADDRESS, _cmd, _length, false); + if(w_status == 0) { + return true; + } else { + pc_.printf("write register: %x \n", _cmd[0]); + pc_.printf(" write: %d \n", w_status); + return false; + } + } + + bool read(char _register, char* _data, int _length) { + char cmd[1] = { _register }; + int w_status = i2c_.write(MPU9250_ADDRESS, cmd, BIT_1, false); + int r_status = i2c_.read(MPU9250_ADDRESS, _data, _length, false); + if(w_status + r_status == 0) { + return true; + } else { + pc_.printf("write register: %x \n", cmd[0]); + pc_.printf("read register: %x \n", _register); + pc_.printf(" write: %d | read: %d \n", w_status, r_status); + return false; + } + }; +}; + +#endif /* MYMPU9250_H */