Diff: MPU_9250/MPU9250.h

Revision:

0:03c072adb139

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU_9250/MPU9250.h	Tue Jul 21 21:05:32 2015 +0000
@@ -0,0 +1,407 @@
+/*CODED by Qiyong Mu on 21/06/2014
+kylongmu@msn.com
+
+Modified by Raghid Mardini
+*/
+
+
+#ifndef mpu9250_h
+#define mpu9250_h
+#include "mbed.h"
+
+/** IMU class.
+* Used to communicate with IMU.
+*/
+class mpu9250_spi
+{
+
+    
+  public:
+    /** Create mpu9250_spi class instance
+    * @param mosi SPI MOSI pin for encoder
+    * @param miso SPI MISO pin for encoder
+    * @param sck SPI SCK pin for encoder
+    * @param ncs SPI CS pin for encoder 
+    */
+    mpu9250_spi(PinName mosi, PinName miso, PinName sck, PinName ncs);
+    
+    /** Writes one byte to a register address
+    * @param WriteAddr Register Address
+    * @param WriteData Data
+    * @returns temp_data recieved from SPI
+    */
+    unsigned int WriteReg( uint8_t WriteAddr, uint8_t WriteData );
+    
+    /** Reads one byte from a register address
+    * @param WriteAddr Register Address
+    * @param WriteData Data
+    * @returns temp_data recieved from SPI
+    */
+    unsigned int ReadReg( uint8_t WriteAddr, uint8_t WriteData );
+    
+    /** Reads multiple bytes from a register address
+    * @param ReadAddr Register Address
+    * @param ReadBuf Pointer to an array where data will be written
+    * @param Bytes Number of bytes to read
+    */
+    void ReadRegs( uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes );
+
+    /** Initializes the IMU
+    * @param sample_rate_div sample rate divider (raging from 0 to 255)
+    * @param low_pass_filter low pass filter value; suitable values are:
+    *             BITS_DLPF_CFG_256HZ_NOLPF2
+    *             BITS_DLPF_CFG_188HZ
+    *             BITS_DLPF_CFG_98HZ
+    *             BITS_DLPF_CFG_42HZ
+    *             BITS_DLPF_CFG_20HZ
+    *             BITS_DLPF_CFG_10HZ
+    *             BITS_DLPF_CFG_5HZ
+    *             BITS_DLPF_CFG_2100HZ_NOLPF
+    * @returns 1 if an error occurred
+    */
+    bool init(int sample_rate_div,int low_pass_filter);
+    
+    /** Reads tempreture reading
+    */
+    void read_temp();
+    /** Reads accelerometer reading
+    */
+    void read_acc();
+    /** Reads gyroscope reading
+    */
+    void read_rot();
+    /** Sets gyroscope scale
+    *  @param scale Gyro scale range. Suitable Values: {BITS_FS_250DPS, BITS_FS_500DPS, BITS_FS_1000DPS, BITS_FS_2000DPS}
+    *  @returns the scale set
+    */
+    unsigned int set_gyro_scale(int scale);
+    
+    /** Sets accelerometer scale
+    *  @param scale Acce scale range. Suitable Values: {BITS_FS_2G, BITS_FS_4G, BITS_FS_8G, BITS_FS_16G}
+    *  @returns the scale set
+    */
+    unsigned int set_acc_scale(int scale);
+    
+    /** Calibrates accelerometer
+    */
+    void calib_acc();
+    
+    /** Calibrates magnetometer
+    */
+    void AK8963_calib_Magnetometer();
+    
+    /** Selects spi slave pin
+    */
+    void select();
+    
+    /** Deselects spi slave pin
+    */
+    void deselect();
+    
+    /** Returns the mpu9250 I2C address
+    * @returns I2C adress of mpu9250
+    */
+    unsigned int whoami();
+    
+    /** Returns the magnetormeter I2C address
+    * @returns I2C adress of magnetometer
+    */
+    uint8_t  AK8963_whoami();
+    
+    /** Reads magnetometer reading
+    */
+    void AK8963_read_Magnetometer();
+    
+    /** Reads all readings from all sensors
+    *
+    */
+    void read_all();
+    
+    /** Calibrates mpu9250 and calculates accelerometer and gyroscope bias
+    */
+    void calibrateMPU9250();    //added
+    
+    /** Calculates quaternions based on Madgwick filter from accelerometer, gyroscope and magnetometer readings
+    * @param ax Accelermoter x reading
+    * @param ay Accelermoter y reading
+    * @param az Accelermoter z reading
+    * @param gx Gyroscope x reading
+    * @param gy Gyroscope y reading
+    * @param gz Gyroscope z reading
+    * @param mx Magnetometer x reading
+    * @param my Magnetometer y reading
+    * @param mz Magnetometer z reading
+    * @param deltat Time diffference since last calculation
+    * @param beta Integration gain
+    */
+    void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz,float deltat,float beta);
+    
+    /** Calculates quaternions based on Madgwick filter from accelerometer and gyroscope readings
+    * @param ax Accelermoter x reading
+    * @param ay Accelermoter y reading
+    * @param az Accelermoter z reading
+    * @param gx Gyroscope x reading
+    * @param gy Gyroscope y reading
+    * @param gz Gyroscope z reading
+    * @param deltat Time diffference since last calculation
+    * @param beta Integration gain
+    */
+    void MadgwickAHRSupdateIMU( float ax, float ay, float az,float gx, float gy, float gz, float deltat,float beta);
+    
+    /** Used in Madgwick filter. Calculates inverse squared value.
+    * @param x input value
+    * @returns inverse squared
+    */
+    float invSqrt(float x);
+    
+    /** Calculates euler angle
+    * @param angle 0=Roll, 1=Pitch, 2=Yaw
+    * @returns Euler angle
+    */
+    float euler_angle(int angle);
+    
+    int calib_data[3];
+    float Magnetometer_ASA[3];
+    float gyroBias[3], accelBias[3]; // Bias corrections for gyro and accelerometer
+    float accelerometer_data[3];
+    float Temperature;
+    float gyroscope_data[3];
+    float Magnetometer[3];
+    float roll,pitch,yaw;
+    float q1,q2,q3,q4;
+    
+  private:
+    SPI _spi;
+    DigitalOut _cs;
+    
+    float acc_divider;
+    float gyro_divider;
+    
+//    PinName _CS_pin;
+//    PinName _SO_pin;
+//    PinName _SCK_pin;
+    float _error;
+};
+
+#endif
+
+
+#define PI 3.14159265
+#define RadToDeg 57.2957795131
+#define DegToRad 0.01745329251
+
+
+// mpu9250 registers
+#define MPUREG_XG_OFFS_TC 0x00
+#define MPUREG_YG_OFFS_TC 0x01
+#define MPUREG_ZG_OFFS_TC 0x02
+#define MPUREG_X_FINE_GAIN 0x03
+#define MPUREG_Y_FINE_GAIN 0x04
+#define MPUREG_Z_FINE_GAIN 0x05
+#define MPUREG_XA_OFFS_H 0x06
+#define MPUREG_XA_OFFS_L 0x07
+#define MPUREG_YA_OFFS_H 0x08
+#define MPUREG_YA_OFFS_L 0x09
+#define MPUREG_ZA_OFFS_H 0x0A
+#define MPUREG_ZA_OFFS_L 0x0B
+#define MPUREG_PRODUCT_ID 0x0C
+#define MPUREG_SELF_TEST_X 0x0D
+#define MPUREG_SELF_TEST_Y 0x0E
+#define MPUREG_SELF_TEST_Z 0x0F
+#define MPUREG_SELF_TEST_A 0x10
+#define MPUREG_XG_OFFS_USRH 0x13
+#define MPUREG_XG_OFFS_USRL 0x14
+#define MPUREG_YG_OFFS_USRH 0x15
+#define MPUREG_YG_OFFS_USRL 0x16
+#define MPUREG_ZG_OFFS_USRH 0x17
+#define MPUREG_ZG_OFFS_USRL 0x18
+#define MPUREG_SMPLRT_DIV 0x19
+#define MPUREG_CONFIG 0x1A
+#define MPUREG_GYRO_CONFIG 0x1B
+#define MPUREG_ACCEL_CONFIG 0x1C
+#define MPUREG_ACCEL_CONFIG_2      0x1D
+#define MPUREG_LP_ACCEL_ODR        0x1E
+#define MPUREG_MOT_THR             0x1F
+#define MPUREG_FIFO_EN             0x23
+#define MPUREG_I2C_MST_CTRL        0x24
+#define MPUREG_I2C_SLV0_ADDR       0x25
+#define MPUREG_I2C_SLV0_REG        0x26
+#define MPUREG_I2C_SLV0_CTRL       0x27
+#define MPUREG_I2C_SLV1_ADDR       0x28
+#define MPUREG_I2C_SLV1_REG        0x29
+#define MPUREG_I2C_SLV1_CTRL       0x2A
+#define MPUREG_I2C_SLV2_ADDR       0x2B
+#define MPUREG_I2C_SLV2_REG        0x2C
+#define MPUREG_I2C_SLV2_CTRL       0x2D
+#define MPUREG_I2C_SLV3_ADDR       0x2E
+#define MPUREG_I2C_SLV3_REG        0x2F
+#define MPUREG_I2C_SLV3_CTRL       0x30
+#define MPUREG_I2C_SLV4_ADDR       0x31
+#define MPUREG_I2C_SLV4_REG        0x32
+#define MPUREG_I2C_SLV4_DO         0x33
+#define MPUREG_I2C_SLV4_CTRL       0x34
+#define MPUREG_I2C_SLV4_DI         0x35
+#define MPUREG_I2C_MST_STATUS      0x36
+#define MPUREG_INT_PIN_CFG 0x37
+#define MPUREG_INT_ENABLE 0x38
+#define MPUREG_ACCEL_XOUT_H 0x3B
+#define MPUREG_ACCEL_XOUT_L 0x3C
+#define MPUREG_ACCEL_YOUT_H 0x3D
+#define MPUREG_ACCEL_YOUT_L 0x3E
+#define MPUREG_ACCEL_ZOUT_H 0x3F
+#define MPUREG_ACCEL_ZOUT_L 0x40
+#define MPUREG_TEMP_OUT_H 0x41
+#define MPUREG_TEMP_OUT_L 0x42
+#define MPUREG_GYRO_XOUT_H 0x43
+#define MPUREG_GYRO_XOUT_L 0x44
+#define MPUREG_GYRO_YOUT_H 0x45
+#define MPUREG_GYRO_YOUT_L 0x46
+#define MPUREG_GYRO_ZOUT_H 0x47
+#define MPUREG_GYRO_ZOUT_L 0x48
+#define MPUREG_EXT_SENS_DATA_00    0x49
+#define MPUREG_EXT_SENS_DATA_01    0x4A
+#define MPUREG_EXT_SENS_DATA_02    0x4B
+#define MPUREG_EXT_SENS_DATA_03    0x4C
+#define MPUREG_EXT_SENS_DATA_04    0x4D
+#define MPUREG_EXT_SENS_DATA_05    0x4E
+#define MPUREG_EXT_SENS_DATA_06    0x4F
+#define MPUREG_EXT_SENS_DATA_07    0x50
+#define MPUREG_EXT_SENS_DATA_08    0x51
+#define MPUREG_EXT_SENS_DATA_09    0x52
+#define MPUREG_EXT_SENS_DATA_10    0x53
+#define MPUREG_EXT_SENS_DATA_11    0x54
+#define MPUREG_EXT_SENS_DATA_12    0x55
+#define MPUREG_EXT_SENS_DATA_13    0x56
+#define MPUREG_EXT_SENS_DATA_14    0x57
+#define MPUREG_EXT_SENS_DATA_15    0x58
+#define MPUREG_EXT_SENS_DATA_16    0x59
+#define MPUREG_EXT_SENS_DATA_17    0x5A
+#define MPUREG_EXT_SENS_DATA_18    0x5B
+#define MPUREG_EXT_SENS_DATA_19    0x5C
+#define MPUREG_EXT_SENS_DATA_20    0x5D
+#define MPUREG_EXT_SENS_DATA_21    0x5E
+#define MPUREG_EXT_SENS_DATA_22    0x5F
+#define MPUREG_EXT_SENS_DATA_23    0x60
+#define MPUREG_I2C_SLV0_DO         0x63
+#define MPUREG_I2C_SLV1_DO         0x64
+#define MPUREG_I2C_SLV2_DO         0x65
+#define MPUREG_I2C_SLV3_DO         0x66
+#define MPUREG_I2C_MST_DELAY_CTRL  0x67
+#define MPUREG_SIGNAL_PATH_RESET   0x68
+#define MPUREG_MOT_DETECT_CTRL     0x69
+#define MPUREG_USER_CTRL 0x6A
+#define MPUREG_PWR_MGMT_1 0x6B
+#define MPUREG_PWR_MGMT_2 0x6C
+#define MPUREG_BANK_SEL 0x6D
+#define MPUREG_MEM_START_ADDR 0x6E
+#define MPUREG_MEM_R_W 0x6F
+#define MPUREG_DMP_CFG_1 0x70
+#define MPUREG_DMP_CFG_2 0x71
+#define MPUREG_FIFO_COUNTH 0x72
+#define MPUREG_FIFO_COUNTL 0x73
+#define MPUREG_FIFO_R_W 0x74
+#define MPUREG_WHOAMI 0x75
+#define MPUREG_XA_OFFSET_H         0x77
+#define MPUREG_XA_OFFSET_L         0x78
+#define MPUREG_YA_OFFSET_H         0x7A
+#define MPUREG_YA_OFFSET_L         0x7B
+#define MPUREG_ZA_OFFSET_H         0x7D
+#define MPUREG_ZA_OFFSET_L         0x7E
+/* ---- AK8963 Reg In MPU9250 ----------------------------------------------- */
+
+#define AK8963_I2C_ADDR             0x0c//0x18
+#define AK8963_Device_ID            0x48
+
+// Read-only Reg
+#define AK8963_WIA                  0x00
+#define AK8963_INFO                 0x01
+#define AK8963_ST1                  0x02
+#define AK8963_HXL                  0x03
+#define AK8963_HXH                  0x04
+#define AK8963_HYL                  0x05
+#define AK8963_HYH                  0x06
+#define AK8963_HZL                  0x07
+#define AK8963_HZH                  0x08
+#define AK8963_ST2                  0x09
+// Write/Read Reg
+#define AK8963_CNTL1                0x0A
+#define AK8963_CNTL2                0x0B
+#define AK8963_ASTC                 0x0C
+#define AK8963_TS1                  0x0D
+#define AK8963_TS2                  0x0E
+#define AK8963_I2CDIS               0x0F
+// Read-only Reg ( ROM )
+#define AK8963_ASAX                 0x10
+#define AK8963_ASAY                 0x11
+#define AK8963_ASAZ                 0x12
+
+// Configuration bits mpu9250
+#define BIT_SLEEP 0x40
+#define BIT_H_RESET 0x80
+#define BITS_CLKSEL 0x07
+#define MPU_CLK_SEL_PLLGYROX 0x01
+#define MPU_CLK_SEL_PLLGYROZ 0x03
+#define MPU_EXT_SYNC_GYROX 0x02
+#define BITS_FS_250DPS              0x00
+#define BITS_FS_500DPS              0x08
+#define BITS_FS_1000DPS             0x10
+#define BITS_FS_2000DPS             0x18
+#define BITS_FS_2G                  0x00
+#define BITS_FS_4G                  0x08
+#define BITS_FS_8G                  0x10
+#define BITS_FS_16G                 0x18
+#define BITS_FS_MASK                0x18
+#define BITS_DLPF_CFG_256HZ_NOLPF2  0x00
+#define BITS_DLPF_CFG_188HZ         0x01
+#define BITS_DLPF_CFG_98HZ          0x02
+#define BITS_DLPF_CFG_42HZ          0x03
+#define BITS_DLPF_CFG_20HZ          0x04
+#define BITS_DLPF_CFG_10HZ          0x05
+#define BITS_DLPF_CFG_5HZ           0x06
+#define BITS_DLPF_CFG_2100HZ_NOLPF  0x07
+#define BITS_DLPF_CFG_MASK          0x07
+#define BIT_INT_ANYRD_2CLEAR        0x10
+#define BIT_RAW_RDY_EN              0x01
+#define BIT_I2C_IF_DIS              0x10
+
+#define READ_FLAG   0x80
+
+/* ---- Sensitivity --------------------------------------------------------- */
+
+#define MPU9250A_2g       ((float)0.000061035156f) // 0.000061035156 g/LSB
+#define MPU9250A_4g       ((float)0.000122070312f) // 0.000122070312 g/LSB
+#define MPU9250A_8g       ((float)0.000244140625f) // 0.000244140625 g/LSB
+#define MPU9250A_16g      ((float)0.000488281250f) // 0.000488281250 g/LSB
+
+#define MPU9250G_250dps   ((float)0.007633587786f) // 0.007633587786 dps/LSB
+#define MPU9250G_500dps   ((float)0.015267175572f) // 0.015267175572 dps/LSB
+#define MPU9250G_1000dps  ((float)0.030487804878f) // 0.030487804878 dps/LSB
+#define MPU9250G_2000dps  ((float)0.060975609756f) // 0.060975609756 dps/LSB
+
+#define MPU9250M_4800uT   ((float)0.6f)            // 0.6 uT/LSB
+
+#define MPU9250T_85degC   ((float)0.002995177763f) // 0.002995177763 degC/LSB
+
+#define     Magnetometer_Sensitivity_Scale_Factor ((float)0.15f)    
+
+// For Calibration Function
+#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
+#define PWR_MGMT_2       0x6C
+#define USER_CTRL        0x6A  // Bit 7 enable DMP, bit 3 reset DMP
+#define CONFIG           0x1A
+#define GYRO_CONFIG      0x1B
+#define ACCEL_CONFIG     0x1C
+#define FIFO_EN          0x23
+#define I2C_MST_CTRL     0x24
+#define FIFO_COUNTH      0x72
+#define FIFO_COUNTL      0x73
+#define FIFO_R_W         0x74
+#define SMPLRT_DIV       0x19
+#define PWR_MGMT_1                  0x6B
+
+/////////////