MPU9250.h

00001 
00002 
00003 #ifndef MPU9250_H
00004 #define MPU9250_H
00005 
00006 
00007 /**
00008  * Includes
00009  */
00010  
00011 #include "mbed.h"
00012 #include "math.h"
00013 
00014 #define USE_ISR 1   // poll or data ready interrupt   
00015 
00016 // See also MPU-9250 Register Map and Descriptions, Revision 4.0, RM-MPU-9250A-00, Rev. 1.4, 9/9/2013 for registers not listed in 
00017 // above document; the MPU9250 and MPU9150 are virtually identical but the latter has a different register map
00018 //
00019 //Magnetometer Registers
00020 #define AK8963_ADDRESS   0x0C<<1
00021 #define WHO_AM_I_AK8963  0x00 // should return 0x48
00022 #define INFO             0x01
00023 #define AK8963_ST1       0x02  // data ready status bit 0
00024 #define AK8963_XOUT_L    0x03  // data
00025 #define AK8963_XOUT_H    0x04
00026 #define AK8963_YOUT_L    0x05
00027 #define AK8963_YOUT_H    0x06
00028 #define AK8963_ZOUT_L    0x07
00029 #define AK8963_ZOUT_H    0x08
00030 #define AK8963_ST2       0x09  // Data overflow bit 3 and data read error status bit 2
00031 #define AK8963_CNTL      0x0A  // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
00032 #define AK8963_ASTC      0x0C  // Self test control
00033 #define AK8963_I2CDIS    0x0F  // I2C disable
00034 #define AK8963_ASAX      0x10  // Fuse ROM x-axis sensitivity adjustment value
00035 #define AK8963_ASAY      0x11  // Fuse ROM y-axis sensitivity adjustment value
00036 #define AK8963_ASAZ      0x12  // Fuse ROM z-axis sensitivity adjustment value
00037 
00038 #define SELF_TEST_X_GYRO 0x00                  
00039 #define SELF_TEST_Y_GYRO 0x01                                                                          
00040 #define SELF_TEST_Z_GYRO 0x02
00041 
00042 /*#define X_FINE_GAIN      0x03 // [7:0] fine gain
00043 #define Y_FINE_GAIN      0x04
00044 #define Z_FINE_GAIN      0x05
00045 #define XA_OFFSET_H      0x06 // User-defined trim values for accelerometer
00046 #define XA_OFFSET_L_TC   0x07
00047 #define YA_OFFSET_H      0x08
00048 #define YA_OFFSET_L_TC   0x09
00049 #define ZA_OFFSET_H      0x0A
00050 #define ZA_OFFSET_L_TC   0x0B */
00051 
00052 #define SELF_TEST_X_ACCEL 0x0D
00053 #define SELF_TEST_Y_ACCEL 0x0E    
00054 #define SELF_TEST_Z_ACCEL 0x0F
00055 
00056 #define SELF_TEST_A      0x10
00057 
00058 #define XG_OFFSET_H      0x13  // User-defined trim values for gyroscope
00059 #define XG_OFFSET_L      0x14
00060 #define YG_OFFSET_H      0x15
00061 #define YG_OFFSET_L      0x16
00062 #define ZG_OFFSET_H      0x17
00063 #define ZG_OFFSET_L      0x18
00064 #define SMPLRT_DIV       0x19
00065 #define CONFIG           0x1A
00066 #define GYRO_CONFIG      0x1B
00067 #define ACCEL_CONFIG     0x1C
00068 #define ACCEL_CONFIG2    0x1D
00069 #define LP_ACCEL_ODR     0x1E   
00070 #define WOM_THR          0x1F   
00071 
00072 #define MOT_DUR          0x20  // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms
00073 #define ZMOT_THR         0x21  // Zero-motion detection threshold bits [7:0]
00074 #define ZRMOT_DUR        0x22  // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms
00075 
00076 #define FIFO_EN          0x23
00077 #define I2C_MST_CTRL     0x24   
00078 #define I2C_SLV0_ADDR    0x25
00079 #define I2C_SLV0_REG     0x26
00080 #define I2C_SLV0_CTRL    0x27
00081 #define I2C_SLV1_ADDR    0x28
00082 #define I2C_SLV1_REG     0x29
00083 #define I2C_SLV1_CTRL    0x2A
00084 #define I2C_SLV2_ADDR    0x2B
00085 #define I2C_SLV2_REG     0x2C
00086 #define I2C_SLV2_CTRL    0x2D
00087 #define I2C_SLV3_ADDR    0x2E
00088 #define I2C_SLV3_REG     0x2F
00089 #define I2C_SLV3_CTRL    0x30
00090 #define I2C_SLV4_ADDR    0x31
00091 #define I2C_SLV4_REG     0x32
00092 #define I2C_SLV4_DO      0x33
00093 #define I2C_SLV4_CTRL    0x34
00094 #define I2C_SLV4_DI      0x35
00095 #define I2C_MST_STATUS   0x36
00096 #define INT_PIN_CFG      0x37
00097 #define INT_ENABLE       0x38
00098 #define DMP_INT_STATUS   0x39  // Check DMP interrupt
00099 #define INT_STATUS       0x3A
00100 #define ACCEL_XOUT_H     0x3B
00101 #define ACCEL_XOUT_L     0x3C
00102 #define ACCEL_YOUT_H     0x3D
00103 #define ACCEL_YOUT_L     0x3E
00104 #define ACCEL_ZOUT_H     0x3F
00105 #define ACCEL_ZOUT_L     0x40
00106 #define TEMP_OUT_H       0x41
00107 #define TEMP_OUT_L       0x42
00108 #define GYRO_XOUT_H      0x43
00109 #define GYRO_XOUT_L      0x44
00110 #define GYRO_YOUT_H      0x45
00111 #define GYRO_YOUT_L      0x46
00112 #define GYRO_ZOUT_H      0x47
00113 #define GYRO_ZOUT_L      0x48
00114 #define EXT_SENS_DATA_00 0x49
00115 #define EXT_SENS_DATA_01 0x4A
00116 #define EXT_SENS_DATA_02 0x4B
00117 #define EXT_SENS_DATA_03 0x4C
00118 #define EXT_SENS_DATA_04 0x4D
00119 #define EXT_SENS_DATA_05 0x4E
00120 #define EXT_SENS_DATA_06 0x4F
00121 #define EXT_SENS_DATA_07 0x50
00122 #define EXT_SENS_DATA_08 0x51
00123 #define EXT_SENS_DATA_09 0x52
00124 #define EXT_SENS_DATA_10 0x53
00125 #define EXT_SENS_DATA_11 0x54
00126 #define EXT_SENS_DATA_12 0x55
00127 #define EXT_SENS_DATA_13 0x56
00128 #define EXT_SENS_DATA_14 0x57
00129 #define EXT_SENS_DATA_15 0x58
00130 #define EXT_SENS_DATA_16 0x59
00131 #define EXT_SENS_DATA_17 0x5A
00132 #define EXT_SENS_DATA_18 0x5B
00133 #define EXT_SENS_DATA_19 0x5C
00134 #define EXT_SENS_DATA_20 0x5D
00135 #define EXT_SENS_DATA_21 0x5E
00136 #define EXT_SENS_DATA_22 0x5F
00137 #define EXT_SENS_DATA_23 0x60
00138 #define MOT_DETECT_STATUS 0x61
00139 #define I2C_SLV0_DO      0x63
00140 #define I2C_SLV1_DO      0x64
00141 #define I2C_SLV2_DO      0x65
00142 #define I2C_SLV3_DO      0x66
00143 #define I2C_MST_DELAY_CTRL 0x67
00144 #define SIGNAL_PATH_RESET  0x68
00145 #define MOT_DETECT_CTRL  0x69
00146 #define USER_CTRL        0x6A  // Bit 7 enable DMP, bit 3 reset DMP
00147 #define PWR_MGMT_1       0x6B // Device defaults to the SLEEP mode
00148 #define PWR_MGMT_2       0x6C
00149 #define DMP_BANK         0x6D  // Activates a specific bank in the DMP
00150 #define DMP_RW_PNT       0x6E  // Set read/write pointer to a specific start address in specified DMP bank
00151 #define DMP_REG          0x6F  // Register in DMP from which to read or to which to write
00152 #define DMP_REG_1        0x70
00153 #define DMP_REG_2        0x71 
00154 #define FIFO_COUNTH      0x72
00155 #define FIFO_COUNTL      0x73
00156 #define FIFO_R_W         0x74
00157 #define WHO_AM_I_MPU9250 0x75 // Should return 0x71
00158 #define XA_OFFSET_H      0x77
00159 #define XA_OFFSET_L      0x78
00160 #define YA_OFFSET_H      0x7A
00161 #define YA_OFFSET_L      0x7B
00162 #define ZA_OFFSET_H      0x7D
00163 #define ZA_OFFSET_L      0x7E
00164 
00165 // Using the MSENSR-9250 breakout board, ADO is set to 0 
00166 // Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1
00167 //mbed uses the eight-bit device address, so shift seven-bit addresses left by one!
00168 #define ADO 0
00169 #if ADO
00170 #define MPU9250_ADDRESS 0x69<<1  // Device address when ADO = 1
00171 #else
00172 #define MPU9250_ADDRESS 0x68<<1  // Device address when ADO = 0
00173 #endif
00174 
00175 extern float aRes, gRes;
00176 
00177 extern Serial pc;  
00178 
00179 
00180 extern float ax,ay,az;
00181 extern float gx,gy,gz;
00182 extern float mx,my,mz;
00183 extern int16_t accelData[3],gyroData[3],tempData;
00184 extern float accelBias[3], gyroBias[3];
00185 extern float magCalibration[3];
00186 extern float magbias[3], magScale[3];
00187 extern int lastUpdate, firstUpdate, Now;
00188 extern float deltat;
00189 extern int delt_t; 
00190 extern int count;
00191 extern float PI;
00192 extern float pitch, yaw, roll;
00193 extern float deltat;                             // integration interval for both filter schemes
00194 extern int lastUpdate;    // used to calculate integration interval                               // used to calculate integration interval
00195 extern float q[4];           // vector to hold quaternion
00196 extern float eInt[3]; 
00197 extern float GyroMeasError;
00198 extern float beta;
00199 extern float GyroMeasDrift;      
00200 extern float zeta; 
00201 extern float SelfTest[6];
00202 
00203 extern uint8_t Ascale;     // AFS_2G, AFS_4G, AFS_8G, AFS_16G
00204 extern uint8_t Gscale; // GFS_250DPS, GFS_500DPS, GFS_1000DPS, GFS_2000DPS
00205 extern uint8_t Mscale; // MFS_14BITS or MFS_16BITS, 14-bit or 16-bit magnetometer resolution
00206 extern uint8_t Mmode;        // Either 8 Hz 0x02) or 100 Hz (0x06) magnetometer data ODR  
00207 extern float aRes, gRes, mRes; 
00208 
00209 
00210 /** MPU9250 IMU library.
00211   *
00212   * Example:
00213   * @code
00214   * Later, maybe
00215   * @endcode
00216   */
00217 class MPU9250 {
00218     public:
00219      /**
00220      * Constructor.
00221      *
00222      * Sleep mode of MPU6050 is immediatly disabled
00223      *
00224      * @param sda - mbed pin to use for the SDA I2C line.
00225      * @param scl - mbed pin to use for the SCL I2C line.
00226      */
00227      MPU9250(PinName sda, PinName scl);
00228      
00229 
00230      /**
00231      * Tests the I2C connection by reading the WHO_AM_I register. 
00232      *
00233      * @return True for a working connection, false for an error
00234      */  
00235      void getAres();  
00236      void getGres(); 
00237      void getMres();
00238           
00239      void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
00240      char readByte(uint8_t address, uint8_t subAddress);
00241      void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);
00242      
00243      void readAccelData(int16_t * destination);
00244      void readGyroData(int16_t * destination);
00245      void readMagData(int16_t * destination);
00246      int16_t readTempData();
00247      
00248      void resetMPU9250();
00249      void initAK8963(float * destination);
00250      void initMPU9250();
00251      void calibrateMPU9250 (float * dest1, float * dest2);
00252      void MPU9250::magcalMPU9250(float * dest1, float * dest2) ;
00253      void MPU9250SelfTest(float * destination);
00254      void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
00255      void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
00256         
00257      private:
00258 
00259      I2C i2c;
00260      char currentAcceleroRange;
00261      char currentGyroRange;
00262      
00263 
00264 };
00265 
00266 
00267 
00268 #endif
00269