Ejemplo de uso del MPU9250 por puerto I2C

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
a0074639
Date:
Sun Oct 25 15:28:55 2015 +0000
Child:
1:ad5417f813f4
Commit message:
comfirmed it works well

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
mpu9250Define.h Show annotated file Show diff for this revision Revisions of this file
myMPU9250.h Show annotated file Show diff for this revision Revisions of this file
--- /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 */