Kiko Ishimoto
MPU9250
Fork of
MPU9250
by 
Kiko Ishimoto
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 /* uint8_t offset[3][2] = 00011 {-21 , MPUREG_XG_OFFS_TC}; 00012 for(i=0; i<offset; i++) { 00013 WriteReg(offset[i][1], offset[i][0]); 00014 wait(0.001); //I2C must slow down the write speed, otherwise it won't work 00015 }*/ 00016 Magnetometer_offset[0]=0; 00017 Magnetometer_offset[1]=0; 00018 Magnetometer_offset[2]=0; 00019 } 00020 00021 unsigned int mpu9250_spi::WriteReg( uint8_t WriteAddr, uint8_t WriteData ) 00022 { 00023 unsigned int temp_val; 00024 select(); 00025 spi.write(WriteAddr); 00026 temp_val=spi.write(WriteData); 00027 deselect(); 00028 wait_us(50); 00029 return temp_val; 00030 } 00031 unsigned int mpu9250_spi::ReadReg( uint8_t WriteAddr, uint8_t WriteData ) 00032 { 00033 return WriteReg(WriteAddr | READ_FLAG,WriteData); 00034 } 00035 void mpu9250_spi::ReadRegs( uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes ) 00036 { 00037 unsigned int i = 0; 00038 00039 select(); 00040 spi.write(ReadAddr | READ_FLAG); 00041 for(i=0; i<Bytes; i++) 00042 ReadBuf[i] = spi.write(0x00); 00043 deselect(); 00044 wait_us(50); 00045 } 00046 00047 /*----------------------------------------------------------------------------------------------- 00048 INITIALIZATION 00049 usage: call this function at startup, giving the sample rate divider (raging from 0 to 255) and 00050 low pass filter value; suitable values are: 00051 BITS_DLPF_CFG_256HZ_NOLPF2 00052 BITS_DLPF_CFG_188HZ 00053 BITS_DLPF_CFG_98HZ 00054 BITS_DLPF_CFG_42HZ 00055 BITS_DLPF_CFG_20HZ 00056 BITS_DLPF_CFG_10HZ 00057 BITS_DLPF_CFG_5HZ 00058 BITS_DLPF_CFG_2100HZ_NOLPF 00059 returns 1 if an error occurred 00060 -----------------------------------------------------------------------------------------------*/ 00061 #define MPU_InitRegNum 17 00062 00063 bool mpu9250_spi::init(int sample_rate_div,int low_pass_filter) 00064 { 00065 uint8_t i = 0; 00066 uint8_t MPU_Init_Data[MPU_InitRegNum][2] = { 00067 {0x80, MPUREG_PWR_MGMT_1}, // Reset Device 00068 {0x01, MPUREG_PWR_MGMT_1}, // Clock Source 00069 {0x00, MPUREG_PWR_MGMT_2}, // Enable Acc & Gyro 00070 {low_pass_filter, MPUREG_CONFIG}, // Use DLPF set Gyroscope bandwidth 184Hz, temperature bandwidth 188Hz 00071 {0x18, MPUREG_GYRO_CONFIG}, // +-2000dps 00072 {0x08, MPUREG_ACCEL_CONFIG}, // +-4G 00073 {0x09, MPUREG_ACCEL_CONFIG_2}, // Set Acc Data Rates, Enable Acc LPF , Bandwidth 184Hz 00074 {0x30, MPUREG_INT_PIN_CFG}, // 00075 //{0x40, MPUREG_I2C_MST_CTRL}, // I2C Speed 348 kHz 00076 //{0x20, MPUREG_USER_CTRL}, // Enable AUX 00077 {0x20, MPUREG_USER_CTRL}, // I2C Master mode 00078 {0x0D, MPUREG_I2C_MST_CTRL}, // I2C configuration multi-master IIC 400KHz 00079 00080 {AK8963_I2C_ADDR, MPUREG_I2C_SLV0_ADDR}, //Set the I2C slave addres of AK8963 and set for write. 00081 //{0x09, MPUREG_I2C_SLV4_CTRL}, 00082 //{0x81, MPUREG_I2C_MST_DELAY_CTRL}, //Enable I2C delay 00083 00084 {AK8963_CNTL2, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer 00085 {0x01, MPUREG_I2C_SLV0_DO}, // Reset AK8963 00086 {0x81, MPUREG_I2C_SLV0_CTRL}, //Enable I2C and set 1 byte 00087 00088 {AK8963_CNTL1, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer 00089 {0x12, MPUREG_I2C_SLV0_DO}, // Register value to continuous measurement in 16bit 00090 {0x81, MPUREG_I2C_SLV0_CTRL} //Enable I2C and set 1 byte 00091 00092 }; 00093 spi.format(8,0); 00094 spi.frequency(1000000); 00095 00096 for(i=0; i<MPU_InitRegNum; i++) { 00097 WriteReg(MPU_Init_Data[i][1], MPU_Init_Data[i][0]); 00098 wait(0.001); //I2C must slow down the write speed, otherwise it won't work 00099 } 00100 00101 set_acc_scale(2); 00102 set_gyro_scale(250); 00103 00104 //AK8963_calib_Magnetometer(); //Can't load this function here , strange problem? 00105 return 0; 00106 } 00107 /*----------------------------------------------------------------------------------------------- 00108 ACCELEROMETER SCALE 00109 usage: call this function at startup, after initialization, to set the right range for the 00110 accelerometers. Suitable ranges are: 00111 BITS_FS_2G 00112 BITS_FS_4G 00113 BITS_FS_8G 00114 BITS_FS_16G 00115 returns the range set (2,4,8 or 16) 00116 -----------------------------------------------------------------------------------------------*/ 00117 unsigned int mpu9250_spi::set_acc_scale(int scale) 00118 { 00119 unsigned int temp_scale; 00120 WriteReg(MPUREG_ACCEL_CONFIG, scale); 00121 00122 switch (scale) { 00123 case BITS_FS_2G: 00124 acc_divider=16384; 00125 break; 00126 case BITS_FS_4G: 00127 acc_divider=8192; 00128 break; 00129 case BITS_FS_8G: 00130 acc_divider=4096; 00131 break; 00132 case BITS_FS_16G: 00133 acc_divider=2048; 00134 break; 00135 } 00136 temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00); 00137 00138 switch (temp_scale) { 00139 case BITS_FS_2G: 00140 temp_scale=2; 00141 break; 00142 case BITS_FS_4G: 00143 temp_scale=4; 00144 break; 00145 case BITS_FS_8G: 00146 temp_scale=8; 00147 break; 00148 case BITS_FS_16G: 00149 temp_scale=16; 00150 break; 00151 } 00152 return temp_scale; 00153 } 00154 00155 00156 /*----------------------------------------------------------------------------------------------- 00157 GYROSCOPE SCALE 00158 usage: call this function at startup, after initialization, to set the right range for the 00159 gyroscopes. Suitable ranges are: 00160 BITS_FS_250DPS 00161 BITS_FS_500DPS 00162 BITS_FS_1000DPS 00163 BITS_FS_2000DPS 00164 returns the range set (250,500,1000 or 2000) 00165 -----------------------------------------------------------------------------------------------*/ 00166 unsigned int mpu9250_spi::set_gyro_scale(int scale) 00167 { 00168 unsigned int temp_scale; 00169 WriteReg(MPUREG_GYRO_CONFIG, scale); 00170 switch (scale) { 00171 case BITS_FS_250DPS: 00172 gyro_divider=131; 00173 break; 00174 case BITS_FS_500DPS: 00175 gyro_divider=65.5; 00176 break; 00177 case BITS_FS_1000DPS: 00178 gyro_divider=32.8; 00179 break; 00180 case BITS_FS_2000DPS: 00181 gyro_divider=16.4; 00182 break; 00183 } 00184 temp_scale=WriteReg(MPUREG_GYRO_CONFIG|READ_FLAG, 0x00); 00185 switch (temp_scale) { 00186 case BITS_FS_250DPS: 00187 temp_scale=250; 00188 break; 00189 case BITS_FS_500DPS: 00190 temp_scale=500; 00191 break; 00192 case BITS_FS_1000DPS: 00193 temp_scale=1000; 00194 break; 00195 case BITS_FS_2000DPS: 00196 temp_scale=2000; 00197 break; 00198 } 00199 return temp_scale; 00200 } 00201 00202 00203 /*----------------------------------------------------------------------------------------------- 00204 WHO AM I? 00205 usage: call this function to know if SPI is working correctly. It checks the I2C address of the 00206 mpu9250 which should be 104 when in SPI mode. 00207 returns the I2C address (104) 00208 -----------------------------------------------------------------------------------------------*/ 00209 unsigned int mpu9250_spi::whoami() 00210 { 00211 unsigned int response; 00212 response=WriteReg(MPUREG_WHOAMI|READ_FLAG, 0x00); 00213 return response; 00214 } 00215 00216 00217 /*----------------------------------------------------------------------------------------------- 00218 READ ACCELEROMETER 00219 usage: call this function to read accelerometer data. Axis represents selected axis: 00220 0 -> X axis 00221 1 -> Y axis 00222 2 -> Z axis 00223 -----------------------------------------------------------------------------------------------*/ 00224 void mpu9250_spi::read_acc() 00225 { 00226 uint8_t response[6]; 00227 int16_t bit_data; 00228 float data; 00229 int i; 00230 ReadRegs(MPUREG_ACCEL_XOUT_H,response,6); 00231 for(i=0; i<3; i++) { 00232 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1]; 00233 data=(float)bit_data; 00234 accelerometer_data[i]=data/acc_divider; 00235 accelerometer_data[i]=0.1*accelerometer_data[i]+0.9*accelerometer_data_prev[i]; 00236 accelerometer_data_prev[i]=accelerometer_data[i]; 00237 } 00238 00239 } 00240 00241 /*----------------------------------------------------------------------------------------------- 00242 READ GYROSCOPE 00243 usage: call this function to read gyroscope data. Axis represents selected axis: 00244 0 -> X axis 00245 1 -> Y axis 00246 2 -> Z axis 00247 -----------------------------------------------------------------------------------------------*/ 00248 void mpu9250_spi::read_rot() 00249 { 00250 uint8_t response[6]; 00251 int16_t bit_data; 00252 float data; 00253 int i; 00254 ReadRegs(MPUREG_GYRO_XOUT_H,response,6); 00255 for(i=0; i<3; i++) { 00256 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1]; 00257 data=(float)bit_data; 00258 gyroscope_data[i]=data; 00259 } 00260 00261 } 00262 00263 /*----------------------------------------------------------------------------------------------- 00264 READ TEMPERATURE 00265 usage: call this function to read temperature data. 00266 returns the value in °C 00267 -----------------------------------------------------------------------------------------------*/ 00268 void mpu9250_spi::read_temp() 00269 { 00270 uint8_t response[2]; 00271 int16_t bit_data; 00272 float data; 00273 ReadRegs(MPUREG_TEMP_OUT_H,response,2); 00274 00275 bit_data=((int16_t)response[0]<<8)|response[1]; 00276 data=(float)bit_data; 00277 Temperature=(data/340)+36.53; 00278 deselect(); 00279 } 00280 00281 /*----------------------------------------------------------------------------------------------- 00282 READ ACCELEROMETER CALIBRATION 00283 usage: call this function to read accelerometer data. Axis represents selected axis: 00284 0 -> X axis 00285 1 -> Y axis 00286 2 -> Z axis 00287 returns Factory Trim value 00288 -----------------------------------------------------------------------------------------------*/ 00289 void mpu9250_spi::calib_acc() 00290 { 00291 uint8_t response[4]; 00292 int temp_scale; 00293 //READ CURRENT ACC SCALE 00294 temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00); 00295 set_acc_scale(BITS_FS_8G); 00296 //ENABLE SELF TEST need modify 00297 //temp_scale=WriteReg(MPUREG_ACCEL_CONFIG, 0x80>>axis); 00298 00299 ReadRegs(MPUREG_SELF_TEST_X,response,4); 00300 calib_data[0]=((response[0]&11100000)>>3)|((response[3]&00110000)>>4); 00301 calib_data[1]=((response[1]&11100000)>>3)|((response[3]&00001100)>>2); 00302 calib_data[2]=((response[2]&11100000)>>3)|((response[3]&00000011)); 00303 00304 set_acc_scale(temp_scale); 00305 } 00306 uint8_t mpu9250_spi::AK8963_whoami() 00307 { 00308 uint8_t response; 00309 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read. 00310 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_WIA); //I2C slave 0 register address from where to begin data transfer 00311 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Read 1 byte from the magnetometer 00312 00313 //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes 00314 wait(0.001); 00315 response=WriteReg(MPUREG_EXT_SENS_DATA_00|READ_FLAG, 0x00); //Read I2C 00316 //ReadRegs(MPUREG_EXT_SENS_DATA_00,response,1); 00317 //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C 00318 00319 return response; 00320 } 00321 double* mpu9250_spi::AK8936_read_Orientation(double *getdata) 00322 { 00323 00324 double gx = accelerometer_data[0]; 00325 double gy = accelerometer_data[1]; 00326 double gz = accelerometer_data[2]; 00327 double mx = Magnetometer[0];//-centorx; 00328 double my = Magnetometer[1];//-centory; 00329 double mz = Magnetometer[2];//-centorz; 00330 double g0x=0.01,g0y=0.001,g0z=0.99; 00331 double Gproduct = gx*g0x+gy*g0y+gz*g0z; 00332 double Glengh = sqrt(gx*gx+gy*gy+gz*gz); 00333 double G0lengh =sqrt(g0x*g0x+g0y*g0y+g0z*g0z); 00334 double nx = (gy * g0z - gz * g0y); 00335 double ny = (gz * g0x - gx * g0z); 00336 double nz = (gx * g0y - gy * g0x); 00337 double GPlengh = sqrt(nx*nx+ny*ny+nz*nz); 00338 nx/=GPlengh; 00339 ny/=GPlengh; 00340 nz/=GPlengh; 00341 double accang = acos((Gproduct)/(fabs(Glengh)*fabs(G0lengh)));/* 00342 double hx=(nx*nx*(1-cos(accang))+cos(accang))*mx + (nx*ny*(1-cos(accang))+nz*sin(accang))*my + (nz*nx*(1-cos(accang))+ny*sin(accang))*mz; 00343 double hy=(nx*ny*(1-cos(accang))+nz*sin(accang))*mx + (ny*ny*(1-cos(accang))+cos(accang))*my + (ny*nz*(1-cos(accang))+nx*sin(accang))*mz;*/ 00344 double Mrot[3][3] = { 00345 {nx*nx*(1-cos(accang))+cos(accang) , nx*ny*(1-cos(accang))+nz*sin(accang) ,nz*nx*(1-cos(accang))+ny*sin(accang)}, 00346 {nx*ny*(1-cos(accang))+nz*sin(accang) , ny*ny*(1-cos(accang))+cos(accang) ,ny*nz*(1-cos(accang))+nx*sin(accang)}, 00347 {nz*nx*(1-cos(accang))+ny*sin(accang) , ny*nz*(1-cos(accang))+nx*sin(accang) ,nz*nz*(1-cos(accang))+cos(accang)} 00348 }; 00349 double MrotD[3][3]; 00350 double temp[3][3]; 00351 for(int i=0; i<3; i++) 00352 for(int j=0; j<3; j++) { 00353 temp[j][i]=Mrot[i][j]; 00354 MrotD[j][i]=temp[j][i]; 00355 } 00356 double hx = MrotD[0][0]*mx+MrotD[0][1]*my+MrotD[0][2]*mz; 00357 double hy = MrotD[1][0]*mx+MrotD[1][1]*my+MrotD[1][2]*mz; 00358 double hz = MrotD[2][0]*mx+MrotD[2][1]*my+MrotD[2][2]*mz; 00359 double heading = atan2(hy,hx)*180/3.14; 00360 if (heading > 0) 00361 heading = 360 - heading; 00362 else 00363 heading = -heading; 00364 float pitch = atan(-hx/sqrt(hy*hy+hz*hz))*180/3.14; //invert gx because +pitch is up. range is +/-90 degrees 00365 float roll = atan(hy/sqrt(hx*hx+hz*hz))*180/3.14; // right side down is +roll 00366 if (gz > 0) 00367 { 00368 if ( roll > 0) 00369 roll = 180 - roll; 00370 else 00371 roll = -180 - roll; 00372 } 00373 getdata[0]=heading; 00374 getdata[1]=pitch; 00375 getdata[2]=roll; 00376 return getdata; 00377 } 00378 void mpu9250_spi::AK8963_setoffset(int x,double offset) 00379 { 00380 if((x<0)||(x>3))return; 00381 Magnetometer_offset[x]+=offset; 00382 } 00383 void mpu9250_spi::AK8963_calib_Magnetometer() 00384 { 00385 uint8_t response[3]; 00386 float data; 00387 int i; 00388 00389 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read. 00390 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_ASAX); //I2C slave 0 register address from where to begin data transfer 00391 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x83); //Read 3 bytes from the magnetometer 00392 00393 //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes 00394 wait(0.001); 00395 //response[0]=WriteReg(MPUREG_EXT_SENS_DATA_01|READ_FLAG, 0x00); //Read I2C 00396 ReadRegs(MPUREG_EXT_SENS_DATA_00,response,3); 00397 00398 //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C 00399 for(i=0; i<3; i++) { 00400 data=response[i]; 00401 Magnetometer_ASA[i]=((data-128)/256+1)*Magnetometer_Sensitivity_Scale_Factor; 00402 } 00403 } 00404 void mpu9250_spi::AK8963_read_Magnetometer() 00405 { 00406 uint8_t response[7]; 00407 int16_t bit_data; 00408 float data; 00409 int i; 00410 00411 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read. 00412 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer 00413 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 6 bytes from the magnetometer 00414 00415 wait(0.001); 00416 ReadRegs(MPUREG_EXT_SENS_DATA_00,response,7); 00417 //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. 00418 for(i=0; i<3; i++) { 00419 bit_data=((int16_t)response[i*2+1]<<8)|response[i*2]; 00420 data=(float)bit_data; 00421 Magnetometer[i]=data*Magnetometer_ASA[i]-Magnetometer_offset[i]; 00422 } 00423 } 00424 void mpu9250_spi::read_all() 00425 { 00426 uint8_t response[21]; 00427 int16_t bit_data; 00428 float data; 00429 int i; 00430 00431 //Send I2C command at first 00432 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read. 00433 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer 00434 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 7 bytes from the magnetometer 00435 //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. 00436 00437 //wait(0.001); 00438 ReadRegs(MPUREG_ACCEL_XOUT_H,response,21); 00439 //Get accelerometer value 00440 for(i=0; i<3; i++) { 00441 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1]; 00442 data=(float)bit_data; 00443 accelerometer_data[i]=data/acc_divider; 00444 //accelerometer_data[i]=0.1*accelerometer_data[i]+0.9*accelerometer_data_prev[i]; 00445 //accelerometer_data_prev[i]=accelerometer_data[i]; 00446 } 00447 //Get temperature 00448 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1]; 00449 data=(float)bit_data; 00450 Temperature=((data-21)/333.87)+21; 00451 //Get gyroscop value 00452 for(i=4; i<7; i++) { 00453 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1]; 00454 data=(float)bit_data; 00455 gyroscope_data[i-4]=data/gyro_divider; 00456 } 00457 //Get Magnetometer value 00458 for(i=7; i<10; i++) { 00459 bit_data=((int16_t)response[i*2+1]<<8)|response[i*2]; 00460 data=(float)bit_data; 00461 Magnetometer[i-7]=data*Magnetometer_ASA[i-7]-Magnetometer_offset[i-7]; 00462 } 00463 } 00464 00465 00466 00467 void mpu9250_spi::SpeedSet() 00468 { 00469 get_speedrate(); 00470 read_acc(); 00471 /*for(int i=0;i<3;i++) 00472 ratespeed[i]=acc[i];*/ 00473 double S[3]; 00474 speedT.stop(); 00475 for(int i=0; i<3; i++) { 00476 00477 S[i] =((double)(ratespeed[i]+prev_ratespeed[i])*speedT.read()/2); 00478 speed[i]+=(double)S[i]; 00479 speed[i]=floor(speed[i]*100.0)/100.0; 00480 Synthesis_speed[i]+=(double)S[i]; 00481 Synthesis_speed[i]=0.9*(double)(Synthesis_speed[i]+(double)ratespeed[i]/100.5)+0.1*ratespeed[i]; 00482 kalman_speed[i]=kalmaspeed[i].getAngle((double)kalman_speed[i], (double)ratespeed[i], (double)angleT.read()*1000); 00483 comp_speed[i]=kalman_speed[i]*0.01+Synthesis_speed[i]*0.01+comp_speed[i]*0.98; 00484 prev_ratespeed[i]=ratespeed[i]; 00485 } 00486 speedT.reset(); 00487 speedT.start(); 00488 } 00489 void mpu9250_spi::MeterSet() 00490 { 00491 double S[3]; 00492 for(int i = 0 ; i<3 ; i++) 00493 ratemeter[i]=comp_speed[i]*1000; 00494 meterT.stop(); 00495 for(int i=0; i<3; i++) { 00496 00497 S[i] =((double)(ratemeter[i]+prev_ratemeter[i])*meterT.read()/2); 00498 meter[i]+=(double)S[i]; 00499 Synthesis_meter[i]+=(double)S[i]; 00500 Synthesis_meter[i]=0.9*(double)(Synthesis_meter[i]+(double)ratemeter[i]/100.5)+0.1*meter[i]; 00501 kalman_meter[i]=kalmameter[i].getAngle((double)kalman_meter[i], (double)ratemeter[i], (double)angleT.read()*1000); 00502 comp_meter[i]=kalman_meter[i]*0.01+Synthesis_meter[i]*0.01+comp_meter[i]*0.98; 00503 prev_ratemeter[i]=ratemeter[i]; 00504 } 00505 meterT.reset(); 00506 meterT.start(); 00507 } 00508 00509 00510 00511 void mpu9250_spi::MPU_setup() 00512 { 00513 z_offset=2; 00514 x_offset=0; 00515 y_offset=0; 00516 uint8_t buffer[6]; 00517 00518 for(int i=0; i<sampleNum; i++) { 00519 ReadRegs(MPUREG_ACCEL_XOUT_H,buffer,6); 00520 int Xacc = (int)buffer[1] << 8 | (int)buffer[0]; 00521 int Yacc = (int)buffer[3] << 8 | (int)buffer[2]; 00522 int Zacc = (int)buffer[5] << 8 | (int)buffer[4]-255; 00523 if((int)Xacc-x_offset>xnoise) 00524 xnoise=(int)Xacc-x_offset; 00525 else if((int)Xacc-x_offset<-xnoise) 00526 xnoise=-(int)Xacc-x_offset; 00527 00528 if((int)Yacc-y_offset>ynoise) 00529 ynoise=(int)Yacc-y_offset; 00530 else if((int)Yacc-y_offset<-ynoise) 00531 ynoise=-(int)Yacc-y_offset; 00532 00533 if((int)Zacc-z_offset>znoise) 00534 znoise=(int)Zacc-z_offset; 00535 else if((int)Zacc-z_offset<-znoise) 00536 znoise=-(int)Zacc-z_offset; 00537 } 00538 00539 00540 } 00541 void mpu9250_spi::MPU_setnum(int Num,float time,double rate) 00542 { 00543 sampleNum=Num; 00544 sampleTime=time; 00545 Rate=rate; 00546 } 00547 00548 00549 void mpu9250_spi::get_angle_acc() 00550 { 00551 //short data[3]; 00552 //get_axis_acc(data); 00553 float R = sqrt(pow((float)accelerometer_data[0],2) + pow((float)accelerometer_data[1],2) + pow((float)accelerometer_data[2],2)); 00554 angle_acc[0]=atan2(accelerometer_data[0],accelerometer_data[2])*(180 / 3.1415); 00555 angle_acc[1]=atan2(accelerometer_data[1],accelerometer_data[2])*(180 / 3.1415); 00556 angle_acc[2]=atan2(accelerometer_data[2],accelerometer_data[0])*(180 / 3.1415); 00557 /*angle_acc[1] = -((180 / 3.1415) * acos((float)accelerometer_data[1] / R)-90); // roll - angle of magnetic field vector in x direction 00558 angle_acc[0] = (180 / 3.1415) * acos((float)accelerometer_data[0] / R)-90; // pitch - angle of magnetic field vector in y direction 00559 angle_acc[2] = (180 / 3.1415) * acos((float)accelerometer_data[2] / R); //*/ 00560 /* 00561 angle_acc[0] = atan2(accelerometer_data[0],sqrt(pow((float)accelerometer_data[1],2)+pow((float)accelerometer_data[2],2)))*180/3.1415; 00562 angle_acc[1] = atan2(accelerometer_data[1],sqrt(pow((float)accelerometer_data[0],2)+pow((float)accelerometer_data[2],2)))*180/3.1415; 00563 angle_acc[2] = atan2(sqrt(pow((float)accelerometer_data[1],2)+pow((float)accelerometer_data[2],2)),accelerometer_data[2])*180/3.1415; 00564 */ 00565 00566 } 00567 void mpu9250_spi::get_rate() 00568 { 00569 uint8_t response[6]; 00570 short offset_t[3]= {0,0,0}; 00571 ReadRegs(MPUREG_GYRO_XOUT_H,response,6); 00572 for(int i=0; i<3; i++) { 00573 short bit_data=((int16_t)response[i*2]<<8)|response[i*2+1]; 00574 rate[i]=((double)(bit_data*0.01)-offset_t[i])/gyro_divider;; 00575 //printf("%d",rate[i]); 00576 } 00577 } 00578 00579 void mpu9250_spi::get_speedrate() 00580 { 00581 uint8_t response[6]; 00582 short offset_t[3]= {0,0,0}; 00583 ReadRegs(MPUREG_ACCEL_XOUT_H,response,6); 00584 for(int i=0; i<3; i++) { 00585 short bit_data=((int16_t)response[i*2]<<8)|response[i*2+1]; 00586 ratespeed[i]=(short)(bit_data*0.01)-offset_t[i]; 00587 } 00588 } 00589 void mpu9250_spi::get_angle(double *x,double *y,double *z) 00590 { 00591 *x=(floor(angle[0]*100.0)/100.0); 00592 *y=(floor(angle[1]*100.0)/100.0); 00593 *z=(floor(angle[2]*100.0)/100.0); 00594 } 00595 void mpu9250_spi::set_angle() 00596 { 00597 get_rate(); 00598 read_acc(); 00599 get_angle_acc(); 00600 double S[3]; 00601 angleT.stop(); 00602 for(int i=0; i<3; i++) { 00603 //rate[i]=gyroscope_data[i]*0.01; 00604 double angA=angle_acc[i]; 00605 S[i] =((double)(rate[i]+prev_rate[i])*angleT.read()/2); 00606 S[i]=(floor(S[i]*100.0)/100.0);//-offset_angle[i]; 00607 if(i == 2) angle[i] += S[1]*gyro_divider; 00608 else angle[i] += S[i]*gyro_divider;;//(floor(S[i]*10000.0)/10000.0);//-offset_angle[i]; 00609 //angle[i]+=(double)S[i]; 00610 Synthesis_angle[i]+=(double)angle[i]; 00611 Synthesis_angle[i]=0.92*(double)(Synthesis_angle[i]+(double)rate[i])+0.08*angA; 00612 kalman_angle[i]=kalma[i].getAngle((double)angA, (double)rate[i], (double)angleT.read_ms()); 00613 comp_angle[i]=kalman_angle[i];//Synthesis_angle[i]*0.5+kalman_angle[i]*0+angle[i]*0.3+angle_acc[i]*0.2; 00614 comp_angle[i]=kalman_angle[i]*0.4+Synthesis_angle[i]*0.4+comp_angle[i]*0.2; 00615 prev_rate[i]=rate[i]; 00616 } 00617 angleT.reset(); 00618 angleT.start(); 00619 } 00620 void mpu9250_spi::set_angleoffset() 00621 { 00622 double axis[3],offseta[3]; 00623 offseta[0]=offseta[1]=offseta[2]=0; 00624 offset_angle[0]=0; 00625 offset_angle[1]=0; 00626 offset_angle[2]=0; 00627 for(int i=0; i<sampleNum; i++) { 00628 offseta[0]+=rate[0]; 00629 offseta[1]+=rate[1]; 00630 offseta[2]+=rate[2]; 00631 } 00632 offset_angle[0]=offseta[0]/sampleNum; 00633 offset_angle[1]=offseta[1]/sampleNum; 00634 offset_angle[2]=offseta[2]/sampleNum; 00635 offset_angle[0]=(floor(offset_angle[0]*100.0)/100.0); 00636 offset_angle[1]=(floor(offset_angle[1]*100.0)/100.0); 00637 offset_angle[2]=(floor(offset_angle[2]*100.0)/100.0); 00638 angle[0]=0; 00639 angle[1]=0; 00640 angle[2]=0; 00641 } 00642 void mpu9250_spi::set_noise() 00643 { 00644 short gyro[3]; 00645 noise[0]=noise[1]=noise[2]=0; 00646 uint8_t response[6]; 00647 for(int i=0; i<sampleNum; i++) { 00648 00649 for(int t=0; t<3; t++) { 00650 for(int i=0; i<3; i++) { 00651 short bit_data=((int16_t)response[i*2]<<8)|response[i*2+1]; 00652 gyro[i]=(short)bit_data; 00653 } 00654 if((int)gyro[t]>noise[t]) 00655 noise[t]=(int)gyro[t]; 00656 else if((int)gyro[t]<-noise[t]) 00657 noise[t]=-(int)gyro[t]; 00658 } 00659 } 00660 noise[0]*=sampleTime; 00661 noise[1]*=sampleTime; 00662 noise[2]*=sampleTime; 00663 } 00664 void mpu9250_spi::set_offset() 00665 { 00666 short axis[3],offseta[3]; 00667 uint8_t response[6]; 00668 offseta[0]=0; 00669 offseta[1]=0; 00670 offseta[2]=0; 00671 for(int i=0; i<sampleNum; i++) { 00672 for(int i=0; i<3; i++) { 00673 short bit_data=((int16_t)response[i*2]<<8)|response[i*2+1]; 00674 axis[i]=(short)bit_data; 00675 } 00676 offseta[0]+=axis[0]; 00677 offseta[1]+=axis[1]; 00678 offseta[2]+=axis[2]; 00679 } 00680 offset[0]=offseta[0]/sampleNum; 00681 offset[1]=offseta[1]/sampleNum; 00682 offset[2]=offseta[2]/sampleNum; 00683 } 00684 mpu9250_spi::kalman::kalman(void) 00685 { 00686 P[0][0] = 0; 00687 P[0][1] = 0; 00688 P[1][0] = 0; 00689 P[1][1] = 0; 00690 00691 angle = 0; 00692 bias = 0; 00693 00694 Q_angle = 0.001; 00695 Q_gyroBias = 0.003; 00696 R_angle = 0.03; 00697 } 00698 00699 double mpu9250_spi::kalman::getAngle(double newAngle, double newRate, double dt) 00700 { 00701 //predict the angle according to the gyroscope 00702 rate = newRate - bias; 00703 angle = dt * rate; 00704 //update the error covariance 00705 P[0][0] += dt * (dt * P[1][1] * P[0][1] - P[1][0] + Q_angle); 00706 P[0][1] -= dt * P[1][1]; 00707 P[1][0] -= dt * P[1][1]; 00708 P[1][1] += dt * Q_gyroBias; 00709 //difference between the predicted angle and the accelerometer angle 00710 y = newAngle - angle; 00711 //estimation error 00712 S = P[0][0] + R_angle; 00713 //determine the kalman gain according to the error covariance and the distrust 00714 K[0] = P[0][0]/S; 00715 K[1] = P[1][0]/S; 00716 //adjust the angle according to the kalman gain and the difference with the measurement 00717 angle += K[0] * y; 00718 bias += K[1] * y; 00719 //update the error covariance 00720 P[0][0] -= K[0] * P[0][0]; 00721 P[0][1] -= K[0] * P[0][1]; 00722 P[1][0] -= K[1] * P[0][0]; 00723 P[1][1] -= K[1] * P[0][1]; 00724 00725 return angle; 00726 } 00727 void mpu9250_spi::kalman::setAngle(double newAngle) 00728 { 00729 angle = newAngle; 00730 }; 00731 void mpu9250_spi::kalman::setQangle(double newQ_angle) 00732 { 00733 Q_angle = newQ_angle; 00734 }; 00735 void mpu9250_spi::kalman::setQgyroBias(double newQ_gyroBias) 00736 { 00737 Q_gyroBias = newQ_gyroBias; 00738 }; 00739 void mpu9250_spi::kalman::setRangle(double newR_angle) 00740 { 00741 R_angle = newR_angle; 00742 }; 00743 00744 double mpu9250_spi::kalman::getRate(void) 00745 { 00746 return rate; 00747 }; 00748 double mpu9250_spi::kalman::getQangle(void) 00749 { 00750 return Q_angle; 00751 }; 00752 double mpu9250_spi::kalman::getQbias(void) 00753 { 00754 return Q_gyroBias; 00755 }; 00756 double mpu9250_spi::kalman::getRangle(void) 00757 { 00758 return R_angle; 00759 }; 00760 00761 /*----------------------------------------------------------------------------------------------- 00762 SPI SELECT AND DESELECT 00763 usage: enable and disable mpu9250 communication bus 00764 -----------------------------------------------------------------------------------------------*/ 00765 void mpu9250_spi::select() 00766 { 00767 //Set CS low to start transmission (interrupts conversion) 00768 cs = 0; 00769 } 00770 void mpu9250_spi::deselect() 00771 { 00772 //Set CS high to stop transmission (restarts conversion) 00773 cs = 1; 00774 }
Generated on Sat Jul 16 2022 09:24:46 by
1.7.2