forked
Fork of MPU9250_SPI by
Embed:
(wiki syntax)
Show/hide line numbers
MPU9250.cpp
00001 /*CODED by Qiyong Mu on 21/06/2014 00002 kylongmu@msn.com 00003 */ 00004 00005 #include <mbed.h> 00006 #include "MPU9250.h" 00007 00008 mpu9250_spi::mpu9250_spi(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs) {} 00009 00010 unsigned int mpu9250_spi::WriteReg( uint8_t WriteAddr, uint8_t WriteData ) 00011 { 00012 unsigned int temp_val; 00013 select(); 00014 spi.write(WriteAddr); 00015 temp_val=spi.write(WriteData); 00016 deselect(); 00017 wait_us(50); 00018 return temp_val; 00019 } 00020 unsigned int mpu9250_spi::ReadReg( uint8_t WriteAddr, uint8_t WriteData ) 00021 { 00022 return WriteReg(WriteAddr | READ_FLAG,WriteData); 00023 } 00024 void mpu9250_spi::ReadRegs( uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes ) 00025 { 00026 unsigned int i = 0; 00027 00028 select(); 00029 spi.write(ReadAddr | READ_FLAG); 00030 for(i=0; i<Bytes; i++) 00031 ReadBuf[i] = spi.write(0x00); 00032 deselect(); 00033 wait_us(50); 00034 } 00035 00036 /*----------------------------------------------------------------------------------------------- 00037 INITIALIZATION 00038 usage: call this function at startup, giving the sample rate divider (raging from 0 to 255) and 00039 low pass filter value; suitable values are: 00040 BITS_DLPF_CFG_256HZ_NOLPF2 00041 BITS_DLPF_CFG_188HZ 00042 BITS_DLPF_CFG_98HZ 00043 BITS_DLPF_CFG_42HZ 00044 BITS_DLPF_CFG_20HZ 00045 BITS_DLPF_CFG_10HZ 00046 BITS_DLPF_CFG_5HZ 00047 BITS_DLPF_CFG_2100HZ_NOLPF 00048 returns 1 if an error occurred 00049 -----------------------------------------------------------------------------------------------*/ 00050 #define MPU_InitRegNum 17 00051 00052 bool mpu9250_spi::init(int sample_rate_div,int low_pass_filter){ 00053 uint8_t i = 0; 00054 uint8_t MPU_Init_Data[MPU_InitRegNum][2] = { 00055 {0x80, MPUREG_PWR_MGMT_1}, // Reset Device 00056 {0x01, MPUREG_PWR_MGMT_1}, // Clock Source 00057 {0x00, MPUREG_PWR_MGMT_2}, // Enable Acc & Gyro 00058 {low_pass_filter, MPUREG_CONFIG}, // Use DLPF set Gyroscope bandwidth 184Hz, temperature bandwidth 188Hz 00059 {0x18, MPUREG_GYRO_CONFIG}, // +-2000dps 00060 {0x08, MPUREG_ACCEL_CONFIG}, // +-4G 00061 {0x09, MPUREG_ACCEL_CONFIG_2}, // Set Acc Data Rates, Enable Acc LPF , Bandwidth 184Hz 00062 {0x30, MPUREG_INT_PIN_CFG}, // 00063 //{0x40, MPUREG_I2C_MST_CTRL}, // I2C Speed 348 kHz 00064 //{0x20, MPUREG_USER_CTRL}, // Enable AUX 00065 {0x20, MPUREG_USER_CTRL}, // I2C Master mode 00066 {0x0D, MPUREG_I2C_MST_CTRL}, // I2C configuration multi-master IIC 400KHz 00067 00068 {AK8963_I2C_ADDR, MPUREG_I2C_SLV0_ADDR}, //Set the I2C slave addres of AK8963 and set for write. 00069 //{0x09, MPUREG_I2C_SLV4_CTRL}, 00070 //{0x81, MPUREG_I2C_MST_DELAY_CTRL}, //Enable I2C delay 00071 00072 {AK8963_CNTL2, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer 00073 {0x01, MPUREG_I2C_SLV0_DO}, // Reset AK8963 00074 {0x81, MPUREG_I2C_SLV0_CTRL}, //Enable I2C and set 1 byte 00075 00076 {AK8963_CNTL1, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer 00077 {0x12, MPUREG_I2C_SLV0_DO}, // Register value to continuous measurement in 16bit 00078 {0x81, MPUREG_I2C_SLV0_CTRL} //Enable I2C and set 1 byte 00079 00080 }; 00081 spi.format(8,0); 00082 spi.frequency(1000000); 00083 00084 for(i=0; i<MPU_InitRegNum; i++) { 00085 WriteReg(MPU_Init_Data[i][1], MPU_Init_Data[i][0]); 00086 wait(0.001); //I2C must slow down the write speed, otherwise it won't work 00087 } 00088 00089 set_acc_scale(2); 00090 set_gyro_scale(250); 00091 00092 //AK8963_calib_Magnetometer(); //Can't load this function here , strange problem? 00093 return 0; 00094 } 00095 /*----------------------------------------------------------------------------------------------- 00096 ACCELEROMETER SCALE 00097 usage: call this function at startup, after initialization, to set the right range for the 00098 accelerometers. Suitable ranges are: 00099 BITS_FS_2G 00100 BITS_FS_4G 00101 BITS_FS_8G 00102 BITS_FS_16G 00103 returns the range set (2,4,8 or 16) 00104 -----------------------------------------------------------------------------------------------*/ 00105 unsigned int mpu9250_spi::set_acc_scale(int scale){ 00106 unsigned int temp_scale; 00107 WriteReg(MPUREG_ACCEL_CONFIG, scale); 00108 00109 switch (scale){ 00110 case BITS_FS_2G: 00111 acc_divider=16384; 00112 break; 00113 case BITS_FS_4G: 00114 acc_divider=8192; 00115 break; 00116 case BITS_FS_8G: 00117 acc_divider=4096; 00118 break; 00119 case BITS_FS_16G: 00120 acc_divider=2048; 00121 break; 00122 } 00123 temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00); 00124 00125 switch (temp_scale){ 00126 case BITS_FS_2G: 00127 temp_scale=2; 00128 break; 00129 case BITS_FS_4G: 00130 temp_scale=4; 00131 break; 00132 case BITS_FS_8G: 00133 temp_scale=8; 00134 break; 00135 case BITS_FS_16G: 00136 temp_scale=16; 00137 break; 00138 } 00139 return temp_scale; 00140 } 00141 00142 00143 /*----------------------------------------------------------------------------------------------- 00144 GYROSCOPE SCALE 00145 usage: call this function at startup, after initialization, to set the right range for the 00146 gyroscopes. Suitable ranges are: 00147 BITS_FS_250DPS 00148 BITS_FS_500DPS 00149 BITS_FS_1000DPS 00150 BITS_FS_2000DPS 00151 returns the range set (250,500,1000 or 2000) 00152 -----------------------------------------------------------------------------------------------*/ 00153 unsigned int mpu9250_spi::set_gyro_scale(int scale){ 00154 unsigned int temp_scale; 00155 WriteReg(MPUREG_GYRO_CONFIG, scale); 00156 switch (scale){ 00157 case BITS_FS_250DPS: 00158 gyro_divider=131; 00159 break; 00160 case BITS_FS_500DPS: 00161 gyro_divider=65.5; 00162 break; 00163 case BITS_FS_1000DPS: 00164 gyro_divider=32.8; 00165 break; 00166 case BITS_FS_2000DPS: 00167 gyro_divider=16.4; 00168 break; 00169 } 00170 temp_scale=WriteReg(MPUREG_GYRO_CONFIG|READ_FLAG, 0x00); 00171 switch (temp_scale){ 00172 case BITS_FS_250DPS: 00173 temp_scale=250; 00174 break; 00175 case BITS_FS_500DPS: 00176 temp_scale=500; 00177 break; 00178 case BITS_FS_1000DPS: 00179 temp_scale=1000; 00180 break; 00181 case BITS_FS_2000DPS: 00182 temp_scale=2000; 00183 break; 00184 } 00185 return temp_scale; 00186 } 00187 00188 00189 /*----------------------------------------------------------------------------------------------- 00190 WHO AM I? 00191 usage: call this function to know if SPI is working correctly. It checks the I2C address of the 00192 mpu9250 which should be 104 when in SPI mode. 00193 returns the I2C address (104) 00194 -----------------------------------------------------------------------------------------------*/ 00195 unsigned int mpu9250_spi::whoami(){ 00196 unsigned int response; 00197 response=WriteReg(MPUREG_WHOAMI|READ_FLAG, 0x00); 00198 return response; 00199 } 00200 00201 00202 /*----------------------------------------------------------------------------------------------- 00203 READ ACCELEROMETER 00204 usage: call this function to read accelerometer data. Axis represents selected axis: 00205 0 -> X axis 00206 1 -> Y axis 00207 2 -> Z axis 00208 -----------------------------------------------------------------------------------------------*/ 00209 void mpu9250_spi::read_acc() 00210 { 00211 uint8_t response[6]; 00212 int16_t bit_data; 00213 float data; 00214 int i; 00215 ReadRegs(MPUREG_ACCEL_XOUT_H,response,6); 00216 for(i=0; i<3; i++) { 00217 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1]; 00218 data=(float)bit_data; 00219 accelerometer_data[i]=data/acc_divider; 00220 } 00221 00222 } 00223 00224 /*----------------------------------------------------------------------------------------------- 00225 READ GYROSCOPE 00226 usage: call this function to read gyroscope data. Axis represents selected axis: 00227 0 -> X axis 00228 1 -> Y axis 00229 2 -> Z axis 00230 -----------------------------------------------------------------------------------------------*/ 00231 void mpu9250_spi::read_rot() 00232 { 00233 uint8_t response[6]; 00234 int16_t bit_data; 00235 float data; 00236 int i; 00237 ReadRegs(MPUREG_GYRO_XOUT_H,response,6); 00238 for(i=0; i<3; i++) { 00239 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1]; 00240 data=(float)bit_data; 00241 gyroscope_data[i]=data/gyro_divider; 00242 } 00243 00244 } 00245 00246 /*----------------------------------------------------------------------------------------------- 00247 READ TEMPERATURE 00248 usage: call this function to read temperature data. 00249 returns the value in °C 00250 -----------------------------------------------------------------------------------------------*/ 00251 void mpu9250_spi::read_temp(){ 00252 uint8_t response[2]; 00253 int16_t bit_data; 00254 float data; 00255 ReadRegs(MPUREG_TEMP_OUT_H,response,2); 00256 00257 bit_data=((int16_t)response[0]<<8)|response[1]; 00258 data=(float)bit_data; 00259 Temperature=((double)data/340)+36.53; 00260 deselect(); 00261 } 00262 00263 /*----------------------------------------------------------------------------------------------- 00264 READ ACCELEROMETER CALIBRATION 00265 usage: call this function to read accelerometer data. Axis represents selected axis: 00266 0 -> X axis 00267 1 -> Y axis 00268 2 -> Z axis 00269 returns Factory Trim value 00270 -----------------------------------------------------------------------------------------------*/ 00271 void mpu9250_spi::calib_acc() 00272 { 00273 uint8_t response[4]; 00274 int temp_scale; 00275 //READ CURRENT ACC SCALE 00276 temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00); 00277 set_acc_scale(BITS_FS_8G); 00278 //ENABLE SELF TEST need modify 00279 //temp_scale=WriteReg(MPUREG_ACCEL_CONFIG, 0x80>>axis); 00280 00281 ReadRegs(MPUREG_SELF_TEST_X,response,4); 00282 calib_data[0]=((response[0]&11100000)>>3)|((response[3]&00110000)>>4); 00283 calib_data[1]=((response[1]&11100000)>>3)|((response[3]&00001100)>>2); 00284 calib_data[2]=((response[2]&11100000)>>3)|((response[3]&00000011)); 00285 00286 set_acc_scale(temp_scale); 00287 } 00288 uint8_t mpu9250_spi::AK8963_whoami(){ 00289 uint8_t response; 00290 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read. 00291 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_WIA); //I2C slave 0 register address from where to begin data transfer 00292 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Read 1 byte from the magnetometer 00293 00294 //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes 00295 wait(0.001); 00296 response=WriteReg(MPUREG_EXT_SENS_DATA_00|READ_FLAG, 0x00); //Read I2C 00297 //ReadRegs(MPUREG_EXT_SENS_DATA_00,response,1); 00298 //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C 00299 00300 return response; 00301 } 00302 void mpu9250_spi::AK8963_calib_Magnetometer(){ 00303 uint8_t response[3]; 00304 float data; 00305 int i; 00306 00307 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read. 00308 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_ASAX); //I2C slave 0 register address from where to begin data transfer 00309 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x83); //Read 3 bytes from the magnetometer 00310 00311 //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes 00312 wait(0.001); 00313 //response[0]=WriteReg(MPUREG_EXT_SENS_DATA_01|READ_FLAG, 0x00); //Read I2C 00314 ReadRegs(MPUREG_EXT_SENS_DATA_00,response,3); 00315 00316 //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C 00317 for(i=0; i<3; i++) { 00318 data=response[i]; 00319 Magnetometer_ASA[i]=((data-128)/256+1)*Magnetometer_Sensitivity_Scale_Factor; 00320 } 00321 } 00322 void mpu9250_spi::AK8963_read_Magnetometer(){ 00323 uint8_t response[7]; 00324 int16_t bit_data; 00325 float data; 00326 int i; 00327 00328 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read. 00329 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer 00330 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 6 bytes from the magnetometer 00331 00332 wait(0.001); 00333 ReadRegs(MPUREG_EXT_SENS_DATA_00,response,7); 00334 //must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement. 00335 for(i=0; i<3; i++) { 00336 bit_data=((int16_t)response[i*2+1]<<8)|response[i*2]; 00337 data=(float)bit_data; 00338 Magnetometer[i]=data*Magnetometer_ASA[i]; 00339 } 00340 } 00341 void mpu9250_spi::read_all(){ 00342 uint8_t response[21]; 00343 int16_t bit_data; 00344 float data; 00345 int i; 00346 00347 //Send I2C command at first 00348 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read. 00349 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer 00350 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 7 bytes from the magnetometer 00351 //must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement. 00352 00353 //wait(0.001); 00354 ReadRegs(MPUREG_ACCEL_XOUT_H,response,21); 00355 //Get accelerometer value 00356 for(i=0; i<3; i++) { 00357 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1]; 00358 data=(float)bit_data; 00359 accelerometer_data[i]=data/acc_divider; 00360 } 00361 //Get temperature 00362 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1]; 00363 data=(float)bit_data; 00364 Temperature=(((double)data-21)/333.87)+21; 00365 //Get gyroscop value 00366 for(i=4; i<7; i++) { 00367 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1]; 00368 data=(float)bit_data; 00369 gyroscope_data[i-4]=data/gyro_divider; 00370 } 00371 //Get Magnetometer value 00372 for(i=7; i<10; i++) { 00373 bit_data=((int16_t)response[i*2+1]<<8)|response[i*2]; 00374 data=(float)bit_data; 00375 Magnetometer[i-7]=data*Magnetometer_ASA[i-7]; 00376 } 00377 } 00378 00379 /*----------------------------------------------------------------------------------------------- 00380 SPI SELECT AND DESELECT 00381 usage: enable and disable mpu9250 communication bus 00382 -----------------------------------------------------------------------------------------------*/ 00383 void mpu9250_spi::select() { 00384 //Set CS low to start transmission (interrupts conversion) 00385 cs = 0; 00386 } 00387 void mpu9250_spi::deselect() { 00388 //Set CS high to stop transmission (restarts conversion) 00389 cs = 1; 00390 }
Generated on Wed Jul 13 2022 22:00:57 by 1.7.2