Thread_LPC
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(PinName mosi, PinName miso, PinName sck, PinName ncs): _cs(ncs), _spi(mosi,miso,sck) ,q1(1),q2(0),q3(0),q4(0),roll(0),pitch(0),yaw(0) {} 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 { 00054 uint8_t i = 0; 00055 uint8_t MPU_Init_Data[MPU_InitRegNum][2] = { 00056 {0x80, MPUREG_PWR_MGMT_1}, // Reset Device 00057 {0x01, MPUREG_PWR_MGMT_1}, // Clock Source 00058 {0x00, MPUREG_PWR_MGMT_2}, // Enable Acc & Gyro 00059 {low_pass_filter, MPUREG_CONFIG}, // Use DLPF set Gyroscope bandwidth 184Hz, temperature bandwidth 188Hz 00060 {0x18, MPUREG_GYRO_CONFIG}, // +-2000dps 00061 {0x08, MPUREG_ACCEL_CONFIG}, // +-4G 00062 {0x09, MPUREG_ACCEL_CONFIG_2}, // Set Acc Data Rates, Enable Acc LPF , Bandwidth 184Hz 00063 {0x30, MPUREG_INT_PIN_CFG}, // 00064 //{0x40, MPUREG_I2C_MST_CTRL}, // I2C Speed 348 kHz 00065 //{0x20, MPUREG_USER_CTRL}, // Enable AUX 00066 {0x20, MPUREG_USER_CTRL}, // I2C Master mode 00067 {0x0D, MPUREG_I2C_MST_CTRL}, // I2C configuration multi-master IIC 400KHz 00068 00069 {AK8963_I2C_ADDR, MPUREG_I2C_SLV0_ADDR}, //Set the I2C slave addres of AK8963 and set for write. 00070 //{0x09, MPUREG_I2C_SLV4_CTRL}, 00071 //{0x81, MPUREG_I2C_MST_DELAY_CTRL}, //Enable I2C delay 00072 00073 {AK8963_CNTL2, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer 00074 {0x01, MPUREG_I2C_SLV0_DO}, // Reset AK8963 00075 {0x81, MPUREG_I2C_SLV0_CTRL}, //Enable I2C and set 1 byte 00076 00077 {AK8963_CNTL1, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer 00078 {0x12, MPUREG_I2C_SLV0_DO}, // Register value to continuous measurement in 16bit 00079 {0x81, MPUREG_I2C_SLV0_CTRL} //Enable I2C and set 1 byte 00080 00081 }; 00082 _spi.format(8,0); 00083 _spi.frequency(1000000); 00084 00085 for(i=0; i<MPU_InitRegNum; i++) { 00086 WriteReg(MPU_Init_Data[i][1], MPU_Init_Data[i][0]); 00087 wait(0.001); //I2C must slow down the write speed, otherwise it won't work 00088 } 00089 00090 set_acc_scale(2); 00091 set_gyro_scale(250); 00092 00093 //AK8963_calib_Magnetometer(); //Can't load this function here , strange problem? 00094 _spi.frequency(16000000); 00095 return 0; 00096 } 00097 /*----------------------------------------------------------------------------------------------- 00098 ACCELEROMETER SCALE 00099 usage: call this function at startup, after initialization, to set the right range for the 00100 accelerometers. Suitable ranges are: 00101 BITS_FS_2G 00102 BITS_FS_4G 00103 BITS_FS_8G 00104 BITS_FS_16G 00105 returns the range set (2,4,8 or 16) 00106 -----------------------------------------------------------------------------------------------*/ 00107 unsigned int mpu9250_spi::set_acc_scale(int scale) 00108 { 00109 unsigned int temp_scale; 00110 WriteReg(MPUREG_ACCEL_CONFIG, scale); 00111 00112 switch (scale) { 00113 case BITS_FS_2G: 00114 acc_divider=16384; 00115 break; 00116 case BITS_FS_4G: 00117 acc_divider=8192; 00118 break; 00119 case BITS_FS_8G: 00120 acc_divider=4096; 00121 break; 00122 case BITS_FS_16G: 00123 acc_divider=2048; 00124 break; 00125 } 00126 temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00); 00127 00128 switch (temp_scale) { 00129 case BITS_FS_2G: 00130 temp_scale=2; 00131 break; 00132 case BITS_FS_4G: 00133 temp_scale=4; 00134 break; 00135 case BITS_FS_8G: 00136 temp_scale=8; 00137 break; 00138 case BITS_FS_16G: 00139 temp_scale=16; 00140 break; 00141 } 00142 return temp_scale; 00143 } 00144 00145 00146 /*----------------------------------------------------------------------------------------------- 00147 GYROSCOPE SCALE 00148 usage: call this function at startup, after initialization, to set the right range for the 00149 gyroscopes. Suitable ranges are: 00150 BITS_FS_250DPS 00151 BITS_FS_500DPS 00152 BITS_FS_1000DPS 00153 BITS_FS_2000DPS 00154 returns the range set (250,500,1000 or 2000) 00155 -----------------------------------------------------------------------------------------------*/ 00156 unsigned int mpu9250_spi::set_gyro_scale(int scale) 00157 { 00158 unsigned int temp_scale; 00159 WriteReg(MPUREG_GYRO_CONFIG, scale); 00160 switch (scale) { 00161 case BITS_FS_250DPS: 00162 gyro_divider=131; 00163 break; 00164 case BITS_FS_500DPS: 00165 gyro_divider=65.5; 00166 break; 00167 case BITS_FS_1000DPS: 00168 gyro_divider=32.8; 00169 break; 00170 case BITS_FS_2000DPS: 00171 gyro_divider=16.4; 00172 break; 00173 } 00174 temp_scale=WriteReg(MPUREG_GYRO_CONFIG|READ_FLAG, 0x00); 00175 switch (temp_scale) { 00176 case BITS_FS_250DPS: 00177 temp_scale=250; 00178 break; 00179 case BITS_FS_500DPS: 00180 temp_scale=500; 00181 break; 00182 case BITS_FS_1000DPS: 00183 temp_scale=1000; 00184 break; 00185 case BITS_FS_2000DPS: 00186 temp_scale=2000; 00187 break; 00188 } 00189 return temp_scale; 00190 } 00191 00192 00193 /*----------------------------------------------------------------------------------------------- 00194 WHO AM I? 00195 usage: call this function to know if SPI is working correctly. It checks the I2C address of the 00196 mpu9250 which should be 104 when in SPI mode. 00197 returns the I2C address (104) 00198 -----------------------------------------------------------------------------------------------*/ 00199 unsigned int mpu9250_spi::whoami() 00200 { 00201 unsigned int response; 00202 response=WriteReg(MPUREG_WHOAMI|READ_FLAG, 0x00); 00203 return response; 00204 } 00205 00206 00207 /*----------------------------------------------------------------------------------------------- 00208 READ ACCELEROMETER 00209 usage: call this function to read accelerometer data. Axis represents selected axis: 00210 0 -> X axis 00211 1 -> Y axis 00212 2 -> Z axis 00213 -----------------------------------------------------------------------------------------------*/ 00214 void mpu9250_spi::read_acc() 00215 { 00216 uint8_t response[6]; 00217 int16_t bit_data; 00218 float data; 00219 int i; 00220 ReadRegs(MPUREG_ACCEL_XOUT_H,response,6); 00221 for(i=0; i<3; i++) { 00222 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1]; 00223 data=(float)bit_data; 00224 accelerometer_data[i]=data/acc_divider; 00225 } 00226 00227 } 00228 00229 /*----------------------------------------------------------------------------------------------- 00230 READ GYROSCOPE 00231 usage: call this function to read gyroscope data. Axis represents selected axis: 00232 0 -> X axis 00233 1 -> Y axis 00234 2 -> Z axis 00235 -----------------------------------------------------------------------------------------------*/ 00236 void mpu9250_spi::read_rot() 00237 { 00238 uint8_t response[6]; 00239 int16_t bit_data; 00240 float data; 00241 int i; 00242 ReadRegs(MPUREG_GYRO_XOUT_H,response,6); 00243 for(i=0; i<3; i++) { 00244 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1]; 00245 data=(float)bit_data; 00246 gyroscope_data[i]=data/gyro_divider; 00247 } 00248 00249 } 00250 00251 /*----------------------------------------------------------------------------------------------- 00252 READ TEMPERATURE 00253 usage: call this function to read temperature data. 00254 returns the value in °C 00255 -----------------------------------------------------------------------------------------------*/ 00256 void mpu9250_spi::read_temp() 00257 { 00258 uint8_t response[2]; 00259 int16_t bit_data; 00260 float data; 00261 ReadRegs(MPUREG_TEMP_OUT_H,response,2); 00262 00263 bit_data=((int16_t)response[0]<<8)|response[1]; 00264 data=(float)bit_data; 00265 Temperature=(data/340)+36.53; 00266 deselect(); 00267 } 00268 00269 /*----------------------------------------------------------------------------------------------- 00270 READ ACCELEROMETER CALIBRATION 00271 usage: call this function to read accelerometer data. Axis represents selected axis: 00272 0 -> X axis 00273 1 -> Y axis 00274 2 -> Z axis 00275 returns Factory Trim value 00276 -----------------------------------------------------------------------------------------------*/ 00277 void mpu9250_spi::calib_acc() 00278 { 00279 uint8_t response[4]; 00280 int temp_scale; 00281 //READ CURRENT ACC SCALE 00282 temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00); 00283 set_acc_scale(BITS_FS_8G); 00284 //ENABLE SELF TEST need modify 00285 //temp_scale=WriteReg(MPUREG_ACCEL_CONFIG, 0x80>>axis); 00286 00287 ReadRegs(MPUREG_SELF_TEST_X,response,4); 00288 calib_data[0]=((response[0]&11100000)>>3)|((response[3]&00110000)>>4); 00289 calib_data[1]=((response[1]&11100000)>>3)|((response[3]&00001100)>>2); 00290 calib_data[2]=((response[2]&11100000)>>3)|((response[3]&00000011)); 00291 00292 set_acc_scale(temp_scale); 00293 } 00294 uint8_t mpu9250_spi::AK8963_whoami() 00295 { 00296 uint8_t response; 00297 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read. 00298 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_WIA); //I2C slave 0 register address from where to begin data transfer 00299 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Read 1 byte from the magnetometer 00300 00301 //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes 00302 wait(0.001); 00303 response=WriteReg(MPUREG_EXT_SENS_DATA_00|READ_FLAG, 0x00); //Read I2C 00304 //ReadRegs(MPUREG_EXT_SENS_DATA_00,response,1); 00305 //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C 00306 00307 return response; 00308 } 00309 void mpu9250_spi::AK8963_calib_Magnetometer() 00310 { 00311 uint8_t response[3]; 00312 float data; 00313 int i; 00314 00315 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read. 00316 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_ASAX); //I2C slave 0 register address from where to begin data transfer 00317 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x83); //Read 3 bytes from the magnetometer 00318 00319 //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes 00320 wait(0.001); 00321 //response[0]=WriteReg(MPUREG_EXT_SENS_DATA_01|READ_FLAG, 0x00); //Read I2C 00322 ReadRegs(MPUREG_EXT_SENS_DATA_00,response,3); 00323 00324 //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C 00325 for(i=0; i<3; i++) { 00326 data=response[i]; 00327 Magnetometer_ASA[i]=((data-128)/256+1)*Magnetometer_Sensitivity_Scale_Factor; 00328 } 00329 } 00330 void mpu9250_spi::AK8963_read_Magnetometer() 00331 { 00332 uint8_t response[7]; 00333 int16_t bit_data; 00334 float data; 00335 int i; 00336 00337 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read. 00338 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer 00339 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 6 bytes from the magnetometer 00340 00341 wait(0.001); 00342 ReadRegs(MPUREG_EXT_SENS_DATA_00,response,7); 00343 //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. 00344 for(i=0; i<3; i++) { 00345 bit_data=((int16_t)response[i*2+1]<<8)|response[i*2]; 00346 data=(float)bit_data; 00347 Magnetometer[i]=data*Magnetometer_ASA[i]; 00348 } 00349 } 00350 void mpu9250_spi::read_all() 00351 { 00352 uint8_t response[21]; 00353 int16_t bit_data; 00354 float data; 00355 int i; 00356 00357 //Send I2C command at first 00358 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read. 00359 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer 00360 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 7 bytes from the magnetometer 00361 //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. 00362 00363 //wait(0.001); 00364 ReadRegs(MPUREG_ACCEL_XOUT_H,response,21); 00365 //Get accelerometer value 00366 for(i=0; i<3; i++) { 00367 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1]; 00368 data=(float)bit_data; 00369 accelerometer_data[i]=data/acc_divider-accelBias[i]; 00370 } 00371 //Get temperature 00372 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1]; 00373 data=(float)bit_data; 00374 Temperature=((data-21)/333.87)+21; 00375 //Get gyroscop value 00376 for(i=4; i<7; i++) { 00377 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1]; 00378 data=(float)bit_data; 00379 gyroscope_data[i-4]=data/gyro_divider-gyroBias[i-4]; 00380 } 00381 //Get Magnetometer value 00382 for(i=7; i<10; i++) { 00383 bit_data=((int16_t)response[i*2+1]<<8)|response[i*2]; 00384 data=(float)bit_data; 00385 Magnetometer[i-7]=data*Magnetometer_ASA[i-7]; 00386 } 00387 } 00388 00389 /*----------------------------------------------------------------------------------------------- 00390 SPI SELECT AND DESELECT 00391 usage: enable and disable mpu9250 communication bus 00392 -----------------------------------------------------------------------------------------------*/ 00393 void mpu9250_spi::select() 00394 { 00395 //Set CS low to start transmission (interrupts conversion) 00396 _cs = 0; 00397 } 00398 void mpu9250_spi::deselect() 00399 { 00400 //Set CS high to stop transmission (restarts conversion) 00401 _cs = 1; 00402 } 00403 00404 00405 00406 00407 void mpu9250_spi::calibrateMPU9250() 00408 { 00409 uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data 00410 uint16_t ii, packet_count, fifo_count; 00411 int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; 00412 00413 // reset device, reset all registers, clear gyro and accelerometer bias registers 00414 WriteReg( PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device 00415 wait(0.1); 00416 00417 // get stable time source 00418 // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001 00419 WriteReg( PWR_MGMT_1, 0x01); 00420 WriteReg( PWR_MGMT_2, 0x00); 00421 wait(0.2); 00422 00423 // Configure device for bias calculation 00424 //WriteReg( INT_ENABLE, 0x00); // Disable all interrupts 00425 WriteReg( FIFO_EN, 0x00); // Disable FIFO 00426 WriteReg( PWR_MGMT_1, 0x00); // Turn on internal clock source 00427 WriteReg( I2C_MST_CTRL, 0x00); // Disable I2C master 00428 WriteReg( USER_CTRL, 0x00); // Disable FIFO and I2C master modes 00429 WriteReg( USER_CTRL, 0x0C); // Reset FIFO and DMP 00430 wait(0.015); 00431 00432 // Configure MPU9250 gyro and accelerometer for bias calculation 00433 WriteReg( CONFIG, 0x01); // Set low-pass filter to 188 Hz 00434 WriteReg( SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz 00435 WriteReg( GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity 00436 WriteReg( ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity 00437 00438 uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec 00439 uint16_t accelsensitivity = 16384; // = 16384 LSB/g 00440 00441 // Configure FIFO to capture accelerometer and gyro data for bias calculation 00442 WriteReg( USER_CTRL, 0x40); // Enable FIFO 00443 WriteReg( FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9250) 00444 wait(0.04); // accumulate 40 samples in 80 milliseconds = 480 bytes 00445 00446 // At end of sample accumulation, turn off FIFO sensor read 00447 WriteReg( FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO 00448 ReadRegs(FIFO_COUNTH,&data[0],2); // read FIFO sample count 00449 fifo_count = ((uint16_t)data[0] << 8) | data[1]; 00450 packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging 00451 00452 for (ii = 0; ii < packet_count; ii++) { 00453 int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0}; 00454 ReadRegs( FIFO_R_W, &data[0],12); // read data for averaging 00455 accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO 00456 accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ; 00457 accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ; 00458 gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ; 00459 gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ; 00460 gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ; 00461 00462 accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases 00463 accel_bias[1] += (int32_t) accel_temp[1]; 00464 accel_bias[2] += (int32_t) accel_temp[2]; 00465 gyro_bias[0] += (int32_t) gyro_temp[0]; 00466 gyro_bias[1] += (int32_t) gyro_temp[1]; 00467 gyro_bias[2] += (int32_t) gyro_temp[2]; 00468 00469 } 00470 accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases 00471 accel_bias[1] /= (int32_t) packet_count; 00472 accel_bias[2] /= (int32_t) packet_count; 00473 gyro_bias[0] /= (int32_t) packet_count; 00474 gyro_bias[1] /= (int32_t) packet_count; 00475 gyro_bias[2] /= (int32_t) packet_count; 00476 00477 if(accel_bias[2] > 0L) { 00478 accel_bias[2] -= (int32_t) accelsensitivity; // Remove gravity from the z-axis accelerometer bias calculation 00479 } else { 00480 accel_bias[2] += (int32_t) accelsensitivity; 00481 } 00482 00483 // Construct the gyro biases for to the hardware gyro bias registers, which are reset to zero upon device startup 00484 data[0] = (-gyro_bias[0]/4 >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format 00485 data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases 00486 data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF; 00487 data[3] = (-gyro_bias[1]/4) & 0xFF; 00488 data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF; 00489 data[5] = (-gyro_bias[2]/4) & 0xFF; 00490 00491 // Push gyro biases to hardware registers 00492 /* WriteReg( XG_OFFSET_H, data[0]); 00493 WriteReg( XG_OFFSET_L, data[1]); 00494 WriteReg( YG_OFFSET_H, data[2]); 00495 WriteReg( YG_OFFSET_L, data[3]); 00496 WriteReg( ZG_OFFSET_H, data[4]); 00497 WriteReg( ZG_OFFSET_L, data[5]); 00498 */ 00499 gyroBias[0] = (float) gyro_bias[0]/(float) gyrosensitivity; // construct gyro bias in deg/s for later manual subtraction 00500 gyroBias[1] = (float) gyro_bias[1]/(float) gyrosensitivity; 00501 gyroBias[2] = (float) gyro_bias[2]/(float) gyrosensitivity; 00502 00503 // Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain 00504 // factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold 00505 // non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature 00506 // compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that 00507 // the accelerometer biases calculated above must be divided by 8. 00508 00509 int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases 00510 ReadRegs( XA_OFFSET_H, &data[0], 2); // Read factory accelerometer trim values 00511 accel_bias_reg[0] = (int16_t) ((int16_t)data[0] << 8) | data[1]; 00512 ReadRegs( YA_OFFSET_H, &data[0], 2); 00513 accel_bias_reg[1] = (int16_t) ((int16_t)data[0] << 8) | data[1]; 00514 ReadRegs( ZA_OFFSET_H, &data[0], 2); 00515 accel_bias_reg[2] = (int16_t) ((int16_t)data[0] << 8) | data[1]; 00516 00517 uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers 00518 uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis 00519 00520 for(ii = 0; ii < 3; ii++) { 00521 if(accel_bias_reg[ii] & mask) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit 00522 } 00523 00524 // Construct total accelerometer bias, including calculated average accelerometer bias from above 00525 accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale) 00526 accel_bias_reg[1] -= (accel_bias[1]/8); 00527 accel_bias_reg[2] -= (accel_bias[2]/8); 00528 00529 data[0] = (accel_bias_reg[0] >> 8) & 0xFF; 00530 data[1] = (accel_bias_reg[0]) & 0xFF; 00531 data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers 00532 data[2] = (accel_bias_reg[1] >> 8) & 0xFF; 00533 data[3] = (accel_bias_reg[1]) & 0xFF; 00534 data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers 00535 data[4] = (accel_bias_reg[2] >> 8) & 0xFF; 00536 data[5] = (accel_bias_reg[2]) & 0xFF; 00537 data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers 00538 00539 // Apparently this is not working for the acceleration biases in the MPU-9250 00540 // Are we handling the temperature correction bit properly? 00541 // Push accelerometer biases to hardware registers 00542 /* WriteReg( XA_OFFSET_H, data[0]); 00543 WriteReg( XA_OFFSET_L, data[1]); 00544 WriteReg( YA_OFFSET_H, data[2]); 00545 WriteReg( YA_OFFSET_L, data[3]); 00546 WriteReg( ZA_OFFSET_H, data[4]); 00547 WriteReg( ZA_OFFSET_L, data[5]); 00548 */ 00549 // Output scaled accelerometer biases for manual subtraction in the main program 00550 accelBias[0] = (float)accel_bias[0]/(float)accelsensitivity; 00551 accelBias[1] = (float)accel_bias[1]/(float)accelsensitivity; 00552 accelBias[2] = (float)accel_bias[2]/(float)accelsensitivity; 00553 } 00554 00555 00556 void mpu9250_spi::MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat,float beta) 00557 { 00558 // float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability 00559 float norm; 00560 float hx, hy, _2bx, _2bz; 00561 float s1, s2, s3, s4; 00562 float qDot1, qDot2, qDot3, qDot4; 00563 00564 if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { 00565 MadgwickAHRSupdateIMU(ax, ay, az,gx, gy, gz, deltat,beta); 00566 return; 00567 } 00568 00569 // Auxiliary variables to avoid repeated arithmetic 00570 float _2q1mx; 00571 float _2q1my; 00572 float _2q1mz; 00573 float _2q2mx; 00574 float _4bx; 00575 float _4bz; 00576 float _2q1 = 2.0f * q1; 00577 float _2q2 = 2.0f * q2; 00578 float _2q3 = 2.0f * q3; 00579 float _2q4 = 2.0f * q4; 00580 float _2q1q3 = 2.0f * q1 * q3; 00581 float _2q3q4 = 2.0f * q3 * q4; 00582 float q1q1 = q1 * q1; 00583 float q1q2 = q1 * q2; 00584 float q1q3 = q1 * q3; 00585 float q1q4 = q1 * q4; 00586 float q2q2 = q2 * q2; 00587 float q2q3 = q2 * q3; 00588 float q2q4 = q2 * q4; 00589 float q3q3 = q3 * q3; 00590 float q3q4 = q3 * q4; 00591 float q4q4 = q4 * q4; 00592 00593 // Normalise accelerometer measurement 00594 norm = sqrt(ax * ax + ay * ay + az * az); 00595 if (norm == 0.0f) return; // handle NaN 00596 norm = 1.0f/norm; 00597 ax *= norm; 00598 ay *= norm; 00599 az *= norm; 00600 00601 // Normalise magnetometer measurement 00602 norm = sqrt(mx * mx + my * my + mz * mz); 00603 if (norm == 0.0f) return; // handle NaN 00604 norm = 1.0f/norm; 00605 mx *= norm; 00606 my *= norm; 00607 mz *= norm; 00608 00609 // Reference direction of Earth's magnetic field 00610 _2q1mx = 2.0f * q1 * mx; 00611 _2q1my = 2.0f * q1 * my; 00612 _2q1mz = 2.0f * q1 * mz; 00613 _2q2mx = 2.0f * q2 * mx; 00614 hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4; 00615 hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4; 00616 _2bx = sqrt(hx * hx + hy * hy); 00617 _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4; 00618 _4bx = 2.0f * _2bx; 00619 _4bz = 2.0f * _2bz; 00620 00621 // Gradient decent algorithm corrective step 00622 s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); 00623 s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); 00624 s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); 00625 s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); 00626 norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude 00627 norm = 1.0f/norm; 00628 s1 *= norm; 00629 s2 *= norm; 00630 s3 *= norm; 00631 s4 *= norm; 00632 00633 // Compute rate of change of quaternion 00634 qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1; 00635 qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2; 00636 qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3; 00637 qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4; 00638 00639 // Integrate to yield quaternion 00640 q1 += qDot1 * deltat; 00641 q2 += qDot2 * deltat; 00642 q3 += qDot3 * deltat; 00643 q4 += qDot4 * deltat; 00644 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion 00645 norm = 1.0f/norm; 00646 //q[0] = q1 * norm; 00647 //q[1] = q2 * norm; 00648 //q[2] = q3 * norm; 00649 //q[3] = q4 * norm; 00650 q1 = q1 * norm; 00651 q2 = q2 * norm; 00652 q3 = q3 * norm; 00653 q4 = q4 * norm; 00654 00655 } 00656 00657 00658 void mpu9250_spi::MadgwickAHRSupdateIMU( float ax, float ay, float az,float gx, float gy, float gz, float deltat,float beta) 00659 { 00660 float recipNorm; 00661 float s0, s1, s2, s3; 00662 float qDot1, qDot2, qDot3, qDot4; 00663 float _2q1, _2q2, _2q3, _2q4, _4q1, _4q2, _4q3 ,_8q2, _8q3, q1q1, q2q2, q3q3, q4q4; 00664 00665 // Rate of change of quaternion from gyroscope 00666 qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz); 00667 qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy); 00668 qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx); 00669 qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx); 00670 00671 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) 00672 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { 00673 00674 // Normalise accelerometer measurement 00675 recipNorm = invSqrt(ax * ax + ay * ay + az * az); 00676 ax *= recipNorm; 00677 ay *= recipNorm; 00678 az *= recipNorm; 00679 00680 // Auxiliary variables to avoid repeated arithmetic 00681 _2q1 = 2.0f * q1; 00682 _2q2 = 2.0f * q2; 00683 _2q3 = 2.0f * q3; 00684 _2q4 = 2.0f * q4; 00685 _4q1 = 4.0f * q1; 00686 _4q2 = 4.0f * q2; 00687 _4q3 = 4.0f * q3; 00688 _8q2 = 8.0f * q2; 00689 _8q3 = 8.0f * q3; 00690 q1q1 = q1 * q1; 00691 q2q2 = q2 * q2; 00692 q3q3 = q3 * q3; 00693 q4q4 = q4 * q4; 00694 00695 // Gradient decent algorithm corrective step 00696 s0 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay; 00697 s1 = _4q2 * q4q4 - _2q4 * ax + 4.0f * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az; 00698 s2 = 4.0f * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az; 00699 s3 = 4.0f * q2q2 * q4 - _2q2 * ax + 4.0f * q3q3 * q4 - _2q3 * ay; 00700 recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude 00701 s0 *= recipNorm; 00702 s1 *= recipNorm; 00703 s2 *= recipNorm; 00704 s3 *= recipNorm; 00705 00706 // Apply feedback step 00707 qDot1 -= beta * s0; 00708 qDot2 -= beta * s1; 00709 qDot3 -= beta * s2; 00710 qDot4 -= beta * s3; 00711 } 00712 00713 // Integrate rate of change of quaternion to yield quaternion 00714 q1 += qDot1 * deltat; 00715 q2 += qDot2 * deltat; 00716 q3 += qDot3 * deltat; 00717 q4 += qDot4 * deltat; 00718 00719 // Normalise quaternion 00720 recipNorm = invSqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); 00721 q1 *= recipNorm; 00722 q2 *= recipNorm; 00723 q3 *= recipNorm; 00724 q4 *= recipNorm; 00725 } 00726 00727 float mpu9250_spi::invSqrt(float x) 00728 { 00729 float halfx = 0.5f * x; 00730 float y = x; 00731 long i = *(long*)&y; 00732 i = 0x5f3759df - (i>>1); 00733 y = *(float*)&i; 00734 y = y * (1.5f - (halfx * y * y)); 00735 return y; 00736 } 00737 00738 float mpu9250_spi::euler_angle(int angle) 00739 { 00740 float euler; 00741 if(angle==0) //roll 00742 euler = atan2(2 * (q1 * q2 + q3 * q4), 1 - 2 * (q2*q2 + q3*q3))*RadToDeg; 00743 else if(angle==1) //pitch 00744 euler = asin(2 * (q1 * q3 - q4 * q2))*RadToDeg; 00745 else if(angle==2) //yaw 00746 euler = atan2(2 * (q1 * q4 + q2 * q3), 1 - 2 * (q3*q3 + q4*q4))*RadToDeg; 00747 /* 00748 float q[4]= {imu.q1 ,imu.q2 ,imu.q3, imu.q4}; 00749 yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3])*180.0f / PI-13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 00750 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]))*180.0f / PI; 00751 roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3])*180.0f / PI; 00752 */ 00753 /* 00754 roll = atan2(2 * (imu.q1 * imu.q2 + imu.q3 * imu.q4), 1 - 2 * (imu.q2*imu.q2 + imu.q3*imu.q3))*RadToDeg; 00755 pitch = asin(2 * (imu.q1 * imu.q3 - imu.q4 * imu.q2))*RadToDeg; 00756 yaw = atan2(2 * (imu.q1 * imu.q4 + imu.q2 * imu.q3), 1 - 2 * (imu.q3*imu.q3 + imu.q4*imu.q4))*RadToDeg; 00757 */ 00758 /* 00759 roll = atan2(2 * (q3 * q4-q1 * q2), -1 + 2 * (q1*q1 + q4*q4))*RadToDeg; 00760 pitch=-atan(2*(q2*q4+q1*q3)/sqrt(1-(2*q2*q4+2*q1*q3)*(2*q2*q4+2*q1*q3)))*RadToDeg; 00761 yaw = atan2(2*(q2*q3-q1*q4),2*q1*q1-1+2*q2*q2)*RadToDeg; 00762 */ 00763 00764 return euler; 00765 }
Generated on Sun Jul 17 2022 08:21:43 by
1.7.2