Thread_LPC

Dependents:   Thread_LPC

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers MPU9250.h Source File

MPU9250.h

00001 /*CODED by Qiyong Mu on 21/06/2014
00002 kylongmu@msn.com
00003 
00004 Modified by Raghid Mardini
00005 */
00006 
00007 
00008 #ifndef mpu9250_h
00009 #define mpu9250_h
00010 #include "mbed.h"
00011 
00012 /** IMU class.
00013 * Used to communicate with IMU.
00014 */
00015 class mpu9250_spi
00016 {
00017 
00018     
00019   public:
00020     /** Create mpu9250_spi class instance
00021     * @param mosi SPI MOSI pin for encoder
00022     * @param miso SPI MISO pin for encoder
00023     * @param sck SPI SCK pin for encoder
00024     * @param ncs SPI CS pin for encoder 
00025     */
00026     mpu9250_spi(PinName mosi, PinName miso, PinName sck, PinName ncs);
00027     
00028     /** Writes one byte to a register address
00029     * @param WriteAddr Register Address
00030     * @param WriteData Data
00031     * @returns temp_data recieved from SPI
00032     */
00033     unsigned int WriteReg( uint8_t WriteAddr, uint8_t WriteData );
00034     
00035     /** Reads one byte from a register address
00036     * @param WriteAddr Register Address
00037     * @param WriteData Data
00038     * @returns temp_data recieved from SPI
00039     */
00040     unsigned int ReadReg( uint8_t WriteAddr, uint8_t WriteData );
00041     
00042     /** Reads multiple bytes from a register address
00043     * @param ReadAddr Register Address
00044     * @param ReadBuf Pointer to an array where data will be written
00045     * @param Bytes Number of bytes to read
00046     */
00047     void ReadRegs( uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes );
00048 
00049     /** Initializes the IMU
00050     * @param sample_rate_div sample rate divider (raging from 0 to 255)
00051     * @param low_pass_filter low pass filter value; suitable values are:
00052     *             BITS_DLPF_CFG_256HZ_NOLPF2
00053     *             BITS_DLPF_CFG_188HZ
00054     *             BITS_DLPF_CFG_98HZ
00055     *             BITS_DLPF_CFG_42HZ
00056     *             BITS_DLPF_CFG_20HZ
00057     *             BITS_DLPF_CFG_10HZ
00058     *             BITS_DLPF_CFG_5HZ
00059     *             BITS_DLPF_CFG_2100HZ_NOLPF
00060     * @returns 1 if an error occurred
00061     */
00062     bool init(int sample_rate_div,int low_pass_filter);
00063     
00064     /** Reads tempreture reading
00065     */
00066     void read_temp();
00067     /** Reads accelerometer reading
00068     */
00069     void read_acc();
00070     /** Reads gyroscope reading
00071     */
00072     void read_rot();
00073     /** Sets gyroscope scale
00074     *  @param scale Gyro scale range. Suitable Values: {BITS_FS_250DPS, BITS_FS_500DPS, BITS_FS_1000DPS, BITS_FS_2000DPS}
00075     *  @returns the scale set
00076     */
00077     unsigned int set_gyro_scale(int scale);
00078     
00079     /** Sets accelerometer scale
00080     *  @param scale Acce scale range. Suitable Values: {BITS_FS_2G, BITS_FS_4G, BITS_FS_8G, BITS_FS_16G}
00081     *  @returns the scale set
00082     */
00083     unsigned int set_acc_scale(int scale);
00084     
00085     /** Calibrates accelerometer
00086     */
00087     void calib_acc();
00088     
00089     /** Calibrates magnetometer
00090     */
00091     void AK8963_calib_Magnetometer();
00092     
00093     /** Selects spi slave pin
00094     */
00095     void select();
00096     
00097     /** Deselects spi slave pin
00098     */
00099     void deselect();
00100     
00101     /** Returns the mpu9250 I2C address
00102     * @returns I2C adress of mpu9250
00103     */
00104     unsigned int whoami();
00105     
00106     /** Returns the magnetormeter I2C address
00107     * @returns I2C adress of magnetometer
00108     */
00109     uint8_t  AK8963_whoami();
00110     
00111     /** Reads magnetometer reading
00112     */
00113     void AK8963_read_Magnetometer();
00114     
00115     /** Reads all readings from all sensors
00116     *
00117     */
00118     void read_all();
00119     
00120     /** Calibrates mpu9250 and calculates accelerometer and gyroscope bias
00121     */
00122     void calibrateMPU9250();    //added
00123     
00124     /** Calculates quaternions based on Madgwick filter from accelerometer, gyroscope and magnetometer readings
00125     * @param ax Accelermoter x reading
00126     * @param ay Accelermoter y reading
00127     * @param az Accelermoter z reading
00128     * @param gx Gyroscope x reading
00129     * @param gy Gyroscope y reading
00130     * @param gz Gyroscope z reading
00131     * @param mx Magnetometer x reading
00132     * @param my Magnetometer y reading
00133     * @param mz Magnetometer z reading
00134     * @param deltat Time diffference since last calculation
00135     * @param beta Integration gain
00136     */
00137     void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz,float deltat,float beta);
00138     
00139     /** Calculates quaternions based on Madgwick filter from accelerometer and gyroscope readings
00140     * @param ax Accelermoter x reading
00141     * @param ay Accelermoter y reading
00142     * @param az Accelermoter z reading
00143     * @param gx Gyroscope x reading
00144     * @param gy Gyroscope y reading
00145     * @param gz Gyroscope z reading
00146     * @param deltat Time diffference since last calculation
00147     * @param beta Integration gain
00148     */
00149     void MadgwickAHRSupdateIMU( float ax, float ay, float az,float gx, float gy, float gz, float deltat,float beta);
00150     
00151     /** Used in Madgwick filter. Calculates inverse squared value.
00152     * @param x input value
00153     * @returns inverse squared
00154     */
00155     float invSqrt(float x);
00156     
00157     /** Calculates euler angle
00158     * @param angle 0=Roll, 1=Pitch, 2=Yaw
00159     * @returns Euler angle
00160     */
00161     float euler_angle(int angle);
00162     
00163     int calib_data[3];
00164     float Magnetometer_ASA[3];
00165     float gyroBias[3], accelBias[3]; // Bias corrections for gyro and accelerometer
00166     float accelerometer_data[3];
00167     float Temperature;
00168     float gyroscope_data[3];
00169     float Magnetometer[3];
00170     float roll,pitch,yaw;
00171     float q1,q2,q3,q4;
00172     
00173   private:
00174     SPI _spi;
00175     DigitalOut _cs;
00176     
00177     float acc_divider;
00178     float gyro_divider;
00179     
00180 //    PinName _CS_pin;
00181 //    PinName _SO_pin;
00182 //    PinName _SCK_pin;
00183     float _error;
00184 };
00185 
00186 #endif
00187 
00188 
00189 #define PI 3.14159265
00190 #define RadToDeg 57.2957795131
00191 #define DegToRad 0.01745329251
00192 
00193 
00194 // mpu9250 registers
00195 #define MPUREG_XG_OFFS_TC 0x00
00196 #define MPUREG_YG_OFFS_TC 0x01
00197 #define MPUREG_ZG_OFFS_TC 0x02
00198 #define MPUREG_X_FINE_GAIN 0x03
00199 #define MPUREG_Y_FINE_GAIN 0x04
00200 #define MPUREG_Z_FINE_GAIN 0x05
00201 #define MPUREG_XA_OFFS_H 0x06
00202 #define MPUREG_XA_OFFS_L 0x07
00203 #define MPUREG_YA_OFFS_H 0x08
00204 #define MPUREG_YA_OFFS_L 0x09
00205 #define MPUREG_ZA_OFFS_H 0x0A
00206 #define MPUREG_ZA_OFFS_L 0x0B
00207 #define MPUREG_PRODUCT_ID 0x0C
00208 #define MPUREG_SELF_TEST_X 0x0D
00209 #define MPUREG_SELF_TEST_Y 0x0E
00210 #define MPUREG_SELF_TEST_Z 0x0F
00211 #define MPUREG_SELF_TEST_A 0x10
00212 #define MPUREG_XG_OFFS_USRH 0x13
00213 #define MPUREG_XG_OFFS_USRL 0x14
00214 #define MPUREG_YG_OFFS_USRH 0x15
00215 #define MPUREG_YG_OFFS_USRL 0x16
00216 #define MPUREG_ZG_OFFS_USRH 0x17
00217 #define MPUREG_ZG_OFFS_USRL 0x18
00218 #define MPUREG_SMPLRT_DIV 0x19
00219 #define MPUREG_CONFIG 0x1A
00220 #define MPUREG_GYRO_CONFIG 0x1B
00221 #define MPUREG_ACCEL_CONFIG 0x1C
00222 #define MPUREG_ACCEL_CONFIG_2      0x1D
00223 #define MPUREG_LP_ACCEL_ODR        0x1E
00224 #define MPUREG_MOT_THR             0x1F
00225 #define MPUREG_FIFO_EN             0x23
00226 #define MPUREG_I2C_MST_CTRL        0x24
00227 #define MPUREG_I2C_SLV0_ADDR       0x25
00228 #define MPUREG_I2C_SLV0_REG        0x26
00229 #define MPUREG_I2C_SLV0_CTRL       0x27
00230 #define MPUREG_I2C_SLV1_ADDR       0x28
00231 #define MPUREG_I2C_SLV1_REG        0x29
00232 #define MPUREG_I2C_SLV1_CTRL       0x2A
00233 #define MPUREG_I2C_SLV2_ADDR       0x2B
00234 #define MPUREG_I2C_SLV2_REG        0x2C
00235 #define MPUREG_I2C_SLV2_CTRL       0x2D
00236 #define MPUREG_I2C_SLV3_ADDR       0x2E
00237 #define MPUREG_I2C_SLV3_REG        0x2F
00238 #define MPUREG_I2C_SLV3_CTRL       0x30
00239 #define MPUREG_I2C_SLV4_ADDR       0x31
00240 #define MPUREG_I2C_SLV4_REG        0x32
00241 #define MPUREG_I2C_SLV4_DO         0x33
00242 #define MPUREG_I2C_SLV4_CTRL       0x34
00243 #define MPUREG_I2C_SLV4_DI         0x35
00244 #define MPUREG_I2C_MST_STATUS      0x36
00245 #define MPUREG_INT_PIN_CFG 0x37
00246 #define MPUREG_INT_ENABLE 0x38
00247 #define MPUREG_ACCEL_XOUT_H 0x3B
00248 #define MPUREG_ACCEL_XOUT_L 0x3C
00249 #define MPUREG_ACCEL_YOUT_H 0x3D
00250 #define MPUREG_ACCEL_YOUT_L 0x3E
00251 #define MPUREG_ACCEL_ZOUT_H 0x3F
00252 #define MPUREG_ACCEL_ZOUT_L 0x40
00253 #define MPUREG_TEMP_OUT_H 0x41
00254 #define MPUREG_TEMP_OUT_L 0x42
00255 #define MPUREG_GYRO_XOUT_H 0x43
00256 #define MPUREG_GYRO_XOUT_L 0x44
00257 #define MPUREG_GYRO_YOUT_H 0x45
00258 #define MPUREG_GYRO_YOUT_L 0x46
00259 #define MPUREG_GYRO_ZOUT_H 0x47
00260 #define MPUREG_GYRO_ZOUT_L 0x48
00261 #define MPUREG_EXT_SENS_DATA_00    0x49
00262 #define MPUREG_EXT_SENS_DATA_01    0x4A
00263 #define MPUREG_EXT_SENS_DATA_02    0x4B
00264 #define MPUREG_EXT_SENS_DATA_03    0x4C
00265 #define MPUREG_EXT_SENS_DATA_04    0x4D
00266 #define MPUREG_EXT_SENS_DATA_05    0x4E
00267 #define MPUREG_EXT_SENS_DATA_06    0x4F
00268 #define MPUREG_EXT_SENS_DATA_07    0x50
00269 #define MPUREG_EXT_SENS_DATA_08    0x51
00270 #define MPUREG_EXT_SENS_DATA_09    0x52
00271 #define MPUREG_EXT_SENS_DATA_10    0x53
00272 #define MPUREG_EXT_SENS_DATA_11    0x54
00273 #define MPUREG_EXT_SENS_DATA_12    0x55
00274 #define MPUREG_EXT_SENS_DATA_13    0x56
00275 #define MPUREG_EXT_SENS_DATA_14    0x57
00276 #define MPUREG_EXT_SENS_DATA_15    0x58
00277 #define MPUREG_EXT_SENS_DATA_16    0x59
00278 #define MPUREG_EXT_SENS_DATA_17    0x5A
00279 #define MPUREG_EXT_SENS_DATA_18    0x5B
00280 #define MPUREG_EXT_SENS_DATA_19    0x5C
00281 #define MPUREG_EXT_SENS_DATA_20    0x5D
00282 #define MPUREG_EXT_SENS_DATA_21    0x5E
00283 #define MPUREG_EXT_SENS_DATA_22    0x5F
00284 #define MPUREG_EXT_SENS_DATA_23    0x60
00285 #define MPUREG_I2C_SLV0_DO         0x63
00286 #define MPUREG_I2C_SLV1_DO         0x64
00287 #define MPUREG_I2C_SLV2_DO         0x65
00288 #define MPUREG_I2C_SLV3_DO         0x66
00289 #define MPUREG_I2C_MST_DELAY_CTRL  0x67
00290 #define MPUREG_SIGNAL_PATH_RESET   0x68
00291 #define MPUREG_MOT_DETECT_CTRL     0x69
00292 #define MPUREG_USER_CTRL 0x6A
00293 #define MPUREG_PWR_MGMT_1 0x6B
00294 #define MPUREG_PWR_MGMT_2 0x6C
00295 #define MPUREG_BANK_SEL 0x6D
00296 #define MPUREG_MEM_START_ADDR 0x6E
00297 #define MPUREG_MEM_R_W 0x6F
00298 #define MPUREG_DMP_CFG_1 0x70
00299 #define MPUREG_DMP_CFG_2 0x71
00300 #define MPUREG_FIFO_COUNTH 0x72
00301 #define MPUREG_FIFO_COUNTL 0x73
00302 #define MPUREG_FIFO_R_W 0x74
00303 #define MPUREG_WHOAMI 0x75
00304 #define MPUREG_XA_OFFSET_H         0x77
00305 #define MPUREG_XA_OFFSET_L         0x78
00306 #define MPUREG_YA_OFFSET_H         0x7A
00307 #define MPUREG_YA_OFFSET_L         0x7B
00308 #define MPUREG_ZA_OFFSET_H         0x7D
00309 #define MPUREG_ZA_OFFSET_L         0x7E
00310 /* ---- AK8963 Reg In MPU9250 ----------------------------------------------- */
00311 
00312 #define AK8963_I2C_ADDR             0x0c//0x18
00313 #define AK8963_Device_ID            0x48
00314 
00315 // Read-only Reg
00316 #define AK8963_WIA                  0x00
00317 #define AK8963_INFO                 0x01
00318 #define AK8963_ST1                  0x02
00319 #define AK8963_HXL                  0x03
00320 #define AK8963_HXH                  0x04
00321 #define AK8963_HYL                  0x05
00322 #define AK8963_HYH                  0x06
00323 #define AK8963_HZL                  0x07
00324 #define AK8963_HZH                  0x08
00325 #define AK8963_ST2                  0x09
00326 // Write/Read Reg
00327 #define AK8963_CNTL1                0x0A
00328 #define AK8963_CNTL2                0x0B
00329 #define AK8963_ASTC                 0x0C
00330 #define AK8963_TS1                  0x0D
00331 #define AK8963_TS2                  0x0E
00332 #define AK8963_I2CDIS               0x0F
00333 // Read-only Reg ( ROM )
00334 #define AK8963_ASAX                 0x10
00335 #define AK8963_ASAY                 0x11
00336 #define AK8963_ASAZ                 0x12
00337 
00338 // Configuration bits mpu9250
00339 #define BIT_SLEEP 0x40
00340 #define BIT_H_RESET 0x80
00341 #define BITS_CLKSEL 0x07
00342 #define MPU_CLK_SEL_PLLGYROX 0x01
00343 #define MPU_CLK_SEL_PLLGYROZ 0x03
00344 #define MPU_EXT_SYNC_GYROX 0x02
00345 #define BITS_FS_250DPS              0x00
00346 #define BITS_FS_500DPS              0x08
00347 #define BITS_FS_1000DPS             0x10
00348 #define BITS_FS_2000DPS             0x18
00349 #define BITS_FS_2G                  0x00
00350 #define BITS_FS_4G                  0x08
00351 #define BITS_FS_8G                  0x10
00352 #define BITS_FS_16G                 0x18
00353 #define BITS_FS_MASK                0x18
00354 #define BITS_DLPF_CFG_256HZ_NOLPF2  0x00
00355 #define BITS_DLPF_CFG_188HZ         0x01
00356 #define BITS_DLPF_CFG_98HZ          0x02
00357 #define BITS_DLPF_CFG_42HZ          0x03
00358 #define BITS_DLPF_CFG_20HZ          0x04
00359 #define BITS_DLPF_CFG_10HZ          0x05
00360 #define BITS_DLPF_CFG_5HZ           0x06
00361 #define BITS_DLPF_CFG_2100HZ_NOLPF  0x07
00362 #define BITS_DLPF_CFG_MASK          0x07
00363 #define BIT_INT_ANYRD_2CLEAR        0x10
00364 #define BIT_RAW_RDY_EN              0x01
00365 #define BIT_I2C_IF_DIS              0x10
00366 
00367 #define READ_FLAG   0x80
00368 
00369 /* ---- Sensitivity --------------------------------------------------------- */
00370 
00371 #define MPU9250A_2g       ((float)0.000061035156f) // 0.000061035156 g/LSB
00372 #define MPU9250A_4g       ((float)0.000122070312f) // 0.000122070312 g/LSB
00373 #define MPU9250A_8g       ((float)0.000244140625f) // 0.000244140625 g/LSB
00374 #define MPU9250A_16g      ((float)0.000488281250f) // 0.000488281250 g/LSB
00375 
00376 #define MPU9250G_250dps   ((float)0.007633587786f) // 0.007633587786 dps/LSB
00377 #define MPU9250G_500dps   ((float)0.015267175572f) // 0.015267175572 dps/LSB
00378 #define MPU9250G_1000dps  ((float)0.030487804878f) // 0.030487804878 dps/LSB
00379 #define MPU9250G_2000dps  ((float)0.060975609756f) // 0.060975609756 dps/LSB
00380 
00381 #define MPU9250M_4800uT   ((float)0.6f)            // 0.6 uT/LSB
00382 
00383 #define MPU9250T_85degC   ((float)0.002995177763f) // 0.002995177763 degC/LSB
00384 
00385 #define     Magnetometer_Sensitivity_Scale_Factor ((float)0.15f)    
00386 
00387 // For Calibration Function
00388 #define XA_OFFSET_H      0x77
00389 #define XA_OFFSET_L      0x78
00390 #define YA_OFFSET_H      0x7A
00391 #define YA_OFFSET_L      0x7B
00392 #define ZA_OFFSET_H      0x7D
00393 #define ZA_OFFSET_L      0x7E
00394 #define PWR_MGMT_2       0x6C
00395 #define USER_CTRL        0x6A  // Bit 7 enable DMP, bit 3 reset DMP
00396 #define CONFIG           0x1A
00397 #define GYRO_CONFIG      0x1B
00398 #define ACCEL_CONFIG     0x1C
00399 #define FIFO_EN          0x23
00400 #define I2C_MST_CTRL     0x24
00401 #define FIFO_COUNTH      0x72
00402 #define FIFO_COUNTL      0x73
00403 #define FIFO_R_W         0x74
00404 #define SMPLRT_DIV       0x19
00405 #define PWR_MGMT_1                  0x6B
00406 
00407 /////////////
00408