MPU9250.h

00001 /*CODED by Qiyong Mu on 21/06/2014
00002 kylongmu@msn.com
00003 */
00004 
00005 
00006 #ifndef mpu9250_h
00007 #define mpu9250_h
00008 #include "mbed.h"
00009 
00010 
00011 class mpu9250_spi
00012 {
00013     SPI& spi;
00014     DigitalOut cs;
00015     
00016   public:
00017     mpu9250_spi(SPI& _spi, PinName _cs);
00018     unsigned int WriteReg( uint8_t WriteAddr, uint8_t WriteData );
00019     unsigned int ReadReg( uint8_t WriteAddr, uint8_t WriteData );
00020     void ReadRegs( uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes );
00021     void get_speedrate();
00022     bool init(int sample_rate_div,int low_pass_filter);
00023     void read_temp();
00024     void read_acc();
00025     void read_rot();
00026     unsigned int set_gyro_scale(int scale);
00027     unsigned int set_acc_scale(int scale);
00028     void calib_acc();
00029     void AK8963_calib_Magnetometer();
00030     void select();
00031     void deselect();
00032     unsigned int whoami();
00033     uint8_t  AK8963_whoami();
00034     void AK8963_read_Magnetometer();
00035     void AK8963_setoffset(int x,double offset);
00036     double *AK8936_read_Orientation(double *getdata);
00037 
00038     void read_all();
00039 
00040 
00041     
00042     float acc_divider;
00043     float gyro_divider;
00044     
00045     int calib_data[3];
00046     float Magnetometer_ASA[3];
00047 
00048     double accelerometer_data[3];
00049     double accelerometer_data_prev[3];
00050     float Temperature;
00051     float gyroscope_data[3];
00052     float Magnetometer[3];
00053     float Magnetometer_offset[3];
00054     double speed[3];
00055     double meter[3];
00056     float angle_acc[3];
00057     short offset_gyro[3];
00058     short rate[3];
00059   
00060     void set(float time=0.0001 , float time2=0.001){sampleTimeSpeed=time;sampleTimeMeter=time2;}       
00061         double freefallSpeedSet(); double freefallSpeedGet(); 
00062         double freefallMeterSet(); double freefallMeterGet();
00063         
00064         void SpeedSet();
00065         void MeterSet();
00066         
00067         void Filter();
00068         double VectolGet();double VectolSet();       
00069         
00070         void MPU_setup();
00071         void MPU_setnum(int Num=500,float time=0.0001,double rate=0.00390635);//set data member
00072         
00073         void get_angle_acc();
00074         void get_rate();
00075         void set_angle();//set always [time]
00076         void newset_angle(double ANG_x,double ANG_y,double ANG_z);
00077         void get_angle(double *x,double *y,double *z);
00078 
00079         void set_noise();
00080         void set_offset();
00081         void set_angleoffset();
00082         short ratespeed[3];
00083         short ratemeter[3];
00084         double Synthesis_speed[3],kalman_speed[3],comp_speed[3];
00085         double Synthesis_meter[3],kalman_meter[3],comp_meter[3];  
00086         double angle[3],Synthesis_angle[3],kalman_angle[3],comp_angle[3];
00087   private:
00088     PinName _CS_pin;
00089     PinName _SO_pin;
00090     PinName _SCK_pin;
00091     float _error;
00092     Timer angleT;
00093     Timer speedT;
00094     Timer meterT;
00095 
00096         //ACC ADXL345
00097         int x_acc,y_acc,z_acc,sampleNum;
00098         double x_offset,y_offset,z_offset;
00099         float gx,gy,gz,xnoise,ynoise,znoise;
00100         //GYALO L3GD20
00101         double Rate;
00102         double  sampleTime;       
00103         float noise[3];
00104         short offset[3];short prev_rate[3];
00105     
00106         double t[3];
00107         short tempDATA_ACC[3],tempDATA_ANGLE[3];
00108         double offset_angle[3];
00109 
00110     short prev_ratespeed[3],prev_speed[3];
00111     short prev_ratemeter[3],prev_meter[3];
00112     double real_acc[3];
00113     short acc[3];
00114     double filter_acc[3];
00115       
00116         int sampleTimeSpeed,sampleTimeMeter;
00117         
00118     class kalman
00119         {
00120             public:
00121                 kalman(void);
00122                 double getAngle(double newAngle, double newRate, double dt);
00123                 
00124                 void setAngle(double newAngle);
00125                 void setQangle(double newQ_angle);
00126                 void setQgyroBias(double newQ_gyroBias);
00127                 void setRangle(double newR_angle);
00128                 
00129                 double getRate(void);
00130                 double getQangle(void);
00131                 double getQbias(void);
00132                 double getRangle(void);
00133                 
00134             
00135             private:
00136                 double P[2][2];         //error covariance matrix
00137                 double K[2];            //kalman gain
00138                 double y;               //angle difference
00139                 double S;               //estimation error
00140             
00141                 double rate;            //rate in deg/s
00142                 double angle;           //kalman angle
00143                 double bias;            //kalman gyro bias
00144             
00145                 double Q_angle;         //process noise variance for the angle of the accelerometer
00146                 double Q_gyroBias;      //process noise variance for the gyroscope bias
00147                 double R_angle;         //measurement noise variance 
00148         };   
00149         kalman kalmaspeed[3];
00150         kalman kalmameter[3];
00151               //KALMAN
00152         kalman kalma[3];
00153 };
00154 
00155 #endif
00156 
00157 
00158 
00159 // mpu9250 registers
00160 #define MPUREG_XG_OFFS_TC 0x00
00161 #define MPUREG_YG_OFFS_TC 0x01
00162 #define MPUREG_ZG_OFFS_TC 0x02
00163 #define MPUREG_X_FINE_GAIN 0x03
00164 #define MPUREG_Y_FINE_GAIN 0x04
00165 #define MPUREG_Z_FINE_GAIN 0x05
00166 #define MPUREG_XA_OFFS_H 0x06
00167 #define MPUREG_XA_OFFS_L 0x07
00168 #define MPUREG_YA_OFFS_H 0x08
00169 #define MPUREG_YA_OFFS_L 0x09
00170 #define MPUREG_ZA_OFFS_H 0x0A
00171 #define MPUREG_ZA_OFFS_L 0x0B
00172 #define MPUREG_PRODUCT_ID 0x0C
00173 #define MPUREG_SELF_TEST_X 0x0D
00174 #define MPUREG_SELF_TEST_Y 0x0E
00175 #define MPUREG_SELF_TEST_Z 0x0F
00176 #define MPUREG_SELF_TEST_A 0x10
00177 #define MPUREG_XG_OFFS_USRH 0x13
00178 #define MPUREG_XG_OFFS_USRL 0x14
00179 #define MPUREG_YG_OFFS_USRH 0x15
00180 #define MPUREG_YG_OFFS_USRL 0x16
00181 #define MPUREG_ZG_OFFS_USRH 0x17
00182 #define MPUREG_ZG_OFFS_USRL 0x18
00183 #define MPUREG_SMPLRT_DIV 0x19
00184 #define MPUREG_CONFIG 0x1A
00185 #define MPUREG_GYRO_CONFIG 0x1B
00186 #define MPUREG_ACCEL_CONFIG 0x1C
00187 #define MPUREG_ACCEL_CONFIG_2      0x1D
00188 #define MPUREG_LP_ACCEL_ODR        0x1E
00189 #define MPUREG_MOT_THR             0x1F
00190 #define MPUREG_FIFO_EN             0x23
00191 #define MPUREG_I2C_MST_CTRL        0x24
00192 #define MPUREG_I2C_SLV0_ADDR       0x25
00193 #define MPUREG_I2C_SLV0_REG        0x26
00194 #define MPUREG_I2C_SLV0_CTRL       0x27
00195 #define MPUREG_I2C_SLV1_ADDR       0x28
00196 #define MPUREG_I2C_SLV1_REG        0x29
00197 #define MPUREG_I2C_SLV1_CTRL       0x2A
00198 #define MPUREG_I2C_SLV2_ADDR       0x2B
00199 #define MPUREG_I2C_SLV2_REG        0x2C
00200 #define MPUREG_I2C_SLV2_CTRL       0x2D
00201 #define MPUREG_I2C_SLV3_ADDR       0x2E
00202 #define MPUREG_I2C_SLV3_REG        0x2F
00203 #define MPUREG_I2C_SLV3_CTRL       0x30
00204 #define MPUREG_I2C_SLV4_ADDR       0x31
00205 #define MPUREG_I2C_SLV4_REG        0x32
00206 #define MPUREG_I2C_SLV4_DO         0x33
00207 #define MPUREG_I2C_SLV4_CTRL       0x34
00208 #define MPUREG_I2C_SLV4_DI         0x35
00209 #define MPUREG_I2C_MST_STATUS      0x36
00210 #define MPUREG_INT_PIN_CFG 0x37
00211 #define MPUREG_INT_ENABLE 0x38
00212 #define MPUREG_ACCEL_XOUT_H 0x3B
00213 #define MPUREG_ACCEL_XOUT_L 0x3C
00214 #define MPUREG_ACCEL_YOUT_H 0x3D
00215 #define MPUREG_ACCEL_YOUT_L 0x3E
00216 #define MPUREG_ACCEL_ZOUT_H 0x3F
00217 #define MPUREG_ACCEL_ZOUT_L 0x40
00218 #define MPUREG_TEMP_OUT_H 0x41
00219 #define MPUREG_TEMP_OUT_L 0x42
00220 #define MPUREG_GYRO_XOUT_H 0x43
00221 #define MPUREG_GYRO_XOUT_L 0x44
00222 #define MPUREG_GYRO_YOUT_H 0x45
00223 #define MPUREG_GYRO_YOUT_L 0x46
00224 #define MPUREG_GYRO_ZOUT_H 0x47
00225 #define MPUREG_GYRO_ZOUT_L 0x48
00226 #define MPUREG_EXT_SENS_DATA_00    0x49
00227 #define MPUREG_EXT_SENS_DATA_01    0x4A
00228 #define MPUREG_EXT_SENS_DATA_02    0x4B
00229 #define MPUREG_EXT_SENS_DATA_03    0x4C
00230 #define MPUREG_EXT_SENS_DATA_04    0x4D
00231 #define MPUREG_EXT_SENS_DATA_05    0x4E
00232 #define MPUREG_EXT_SENS_DATA_06    0x4F
00233 #define MPUREG_EXT_SENS_DATA_07    0x50
00234 #define MPUREG_EXT_SENS_DATA_08    0x51
00235 #define MPUREG_EXT_SENS_DATA_09    0x52
00236 #define MPUREG_EXT_SENS_DATA_10    0x53
00237 #define MPUREG_EXT_SENS_DATA_11    0x54
00238 #define MPUREG_EXT_SENS_DATA_12    0x55
00239 #define MPUREG_EXT_SENS_DATA_13    0x56
00240 #define MPUREG_EXT_SENS_DATA_14    0x57
00241 #define MPUREG_EXT_SENS_DATA_15    0x58
00242 #define MPUREG_EXT_SENS_DATA_16    0x59
00243 #define MPUREG_EXT_SENS_DATA_17    0x5A
00244 #define MPUREG_EXT_SENS_DATA_18    0x5B
00245 #define MPUREG_EXT_SENS_DATA_19    0x5C
00246 #define MPUREG_EXT_SENS_DATA_20    0x5D
00247 #define MPUREG_EXT_SENS_DATA_21    0x5E
00248 #define MPUREG_EXT_SENS_DATA_22    0x5F
00249 #define MPUREG_EXT_SENS_DATA_23    0x60
00250 #define MPUREG_I2C_SLV0_DO         0x63
00251 #define MPUREG_I2C_SLV1_DO         0x64
00252 #define MPUREG_I2C_SLV2_DO         0x65
00253 #define MPUREG_I2C_SLV3_DO         0x66
00254 #define MPUREG_I2C_MST_DELAY_CTRL  0x67
00255 #define MPUREG_SIGNAL_PATH_RESET   0x68
00256 #define MPUREG_MOT_DETECT_CTRL     0x69
00257 #define MPUREG_USER_CTRL 0x6A
00258 #define MPUREG_PWR_MGMT_1 0x6B
00259 #define MPUREG_PWR_MGMT_2 0x6C
00260 #define MPUREG_BANK_SEL 0x6D
00261 #define MPUREG_MEM_START_ADDR 0x6E
00262 #define MPUREG_MEM_R_W 0x6F
00263 #define MPUREG_DMP_CFG_1 0x70
00264 #define MPUREG_DMP_CFG_2 0x71
00265 #define MPUREG_FIFO_COUNTH 0x72
00266 #define MPUREG_FIFO_COUNTL 0x73
00267 #define MPUREG_FIFO_R_W 0x74
00268 #define MPUREG_WHOAMI 0x75
00269 #define MPUREG_XA_OFFSET_H         0x77
00270 #define MPUREG_XA_OFFSET_L         0x78
00271 #define MPUREG_YA_OFFSET_H         0x7A
00272 #define MPUREG_YA_OFFSET_L         0x7B
00273 #define MPUREG_ZA_OFFSET_H         0x7D
00274 #define MPUREG_ZA_OFFSET_L         0x7E
00275 /* ---- AK8963 Reg In MPU9250 ----------------------------------------------- */
00276 
00277 #define AK8963_I2C_ADDR             0x0c//0x18
00278 #define AK8963_Device_ID            0x48
00279 
00280 // Read-only Reg
00281 #define AK8963_WIA                  0x00
00282 #define AK8963_INFO                 0x01
00283 #define AK8963_ST1                  0x02
00284 #define AK8963_HXL                  0x03
00285 #define AK8963_HXH                  0x04
00286 #define AK8963_HYL                  0x05
00287 #define AK8963_HYH                  0x06
00288 #define AK8963_HZL                  0x07
00289 #define AK8963_HZH                  0x08
00290 #define AK8963_ST2                  0x09
00291 // Write/Read Reg
00292 #define AK8963_CNTL1                0x0A
00293 #define AK8963_CNTL2                0x0B
00294 #define AK8963_ASTC                 0x0C
00295 #define AK8963_TS1                  0x0D
00296 #define AK8963_TS2                  0x0E
00297 #define AK8963_I2CDIS               0x0F
00298 // Read-only Reg ( ROM )
00299 #define AK8963_ASAX                 0x10
00300 #define AK8963_ASAY                 0x11
00301 #define AK8963_ASAZ                 0x12
00302 
00303 // Configuration bits mpu9250
00304 #define BIT_SLEEP 0x40
00305 #define BIT_H_RESET 0x80
00306 #define BITS_CLKSEL 0x07
00307 #define MPU_CLK_SEL_PLLGYROX 0x01
00308 #define MPU_CLK_SEL_PLLGYROZ 0x03
00309 #define MPU_EXT_SYNC_GYROX 0x02
00310 #define BITS_FS_250DPS              0x00
00311 #define BITS_FS_500DPS              0x08
00312 #define BITS_FS_1000DPS             0x10
00313 #define BITS_FS_2000DPS             0x18
00314 #define BITS_FS_2G                  0x00
00315 #define BITS_FS_4G                  0x08
00316 #define BITS_FS_8G                  0x10
00317 #define BITS_FS_16G                 0x18
00318 #define BITS_FS_MASK                0x18
00319 #define BITS_DLPF_CFG_256HZ_NOLPF2  0x00
00320 #define BITS_DLPF_CFG_188HZ         0x01
00321 #define BITS_DLPF_CFG_98HZ          0x02
00322 #define BITS_DLPF_CFG_42HZ          0x03
00323 #define BITS_DLPF_CFG_20HZ          0x04
00324 #define BITS_DLPF_CFG_10HZ          0x05
00325 #define BITS_DLPF_CFG_5HZ           0x06
00326 #define BITS_DLPF_CFG_2100HZ_NOLPF  0x07
00327 #define BITS_DLPF_CFG_MASK          0x07
00328 #define BIT_INT_ANYRD_2CLEAR        0x10
00329 #define BIT_RAW_RDY_EN              0x01
00330 #define BIT_I2C_IF_DIS              0x10
00331 
00332 #define READ_FLAG   0x80
00333 
00334 /* ---- Sensitivity --------------------------------------------------------- */
00335 
00336 #define MPU9250A_2g       ((float)0.000061035156f) // 0.000061035156 g/LSB
00337 #define MPU9250A_4g       ((float)0.000122070312f) // 0.000122070312 g/LSB
00338 #define MPU9250A_8g       ((float)0.000244140625f) // 0.000244140625 g/LSB
00339 #define MPU9250A_16g      ((float)0.000488281250f) // 0.000488281250 g/LSB
00340 
00341 #define MPU9250G_250dps   ((float)0.007633587786f) // 0.007633587786 dps/LSB
00342 #define MPU9250G_500dps   ((float)0.015267175572f) // 0.015267175572 dps/LSB
00343 #define MPU9250G_1000dps  ((float)0.030487804878f) // 0.030487804878 dps/LSB
00344 #define MPU9250G_2000dps  ((float)0.060975609756f) // 0.060975609756 dps/LSB
00345 
00346 #define MPU9250M_4800uT   ((float)0.6f)            // 0.6 uT/LSB
00347 
00348 #define MPU9250T_85degC   ((float)0.002995177763f) // 0.002995177763 degC/LSB
00349 
00350 #define     Magnetometer_Sensitivity_Scale_Factor ((float)0.15f)