Nora Vazbyte
/
MPU9250
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
MPU9250
by 
Edutech IoT Team
Embed:
(wiki syntax)
Show/hide line numbers
MPU9250.cpp
00001 #include "MPU9250.h" 00002 00003 #define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral 00004 #define Ki 0.0f 00005 00006 //****************************************************************************** 00007 MPU9250::MPU9250(PinName sda, PinName scl) 00008 { 00009 i2c_ = new I2C(sda, scl); 00010 i2c_->frequency(400000); 00011 } 00012 00013 //****************************************************************************** 00014 MPU9250::MPU9250(I2C *i2c):i2c_(i2c){} 00015 00016 //****************************************************************************** 00017 MPU9250::~MPU9250() 00018 { 00019 delete i2c_; 00020 } 00021 00022 void MPU9250::writeByte(uint8_t address, uint8_t subAddress, uint8_t data) 00023 { 00024 char data_write[2]; 00025 data_write[0] = subAddress; 00026 data_write[1] = data; 00027 i2c_->write(address, data_write, 2, 0); 00028 } 00029 00030 char MPU9250::readByte(uint8_t address, uint8_t subAddress) 00031 { 00032 char data[1]; // `data` will store the register data 00033 char data_write[1]; 00034 data_write[0] = subAddress; 00035 i2c_->write(address, data_write, 1, 1); // no stop 00036 i2c_->read(address, data, 1, 0); 00037 return data[0]; 00038 } 00039 00040 void MPU9250::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest) 00041 { 00042 char data[14]; 00043 char data_write[1]; 00044 data_write[0] = subAddress; 00045 i2c_->write(address, data_write, 1, 1); // no stop 00046 i2c_->read(address, data, count, 0); 00047 for(int ii = 0; ii < count; ii++) { 00048 dest[ii] = data[ii]; 00049 } 00050 } 00051 00052 void MPU9250::getMres() { 00053 switch (Mscale) 00054 { 00055 // Possible magnetometer scales (and their register bit settings) are: 00056 // 14 bit resolution (0) and 16 bit resolution (1) 00057 case MFS_14BITS: 00058 mRes = 10.0*4219.0/8190.0; // Proper scale to return milliGauss 00059 break; 00060 case MFS_16BITS: 00061 mRes = 10.0*4219.0/32760.0; // Proper scale to return milliGauss 00062 break; 00063 } 00064 } 00065 00066 00067 void MPU9250::getGres() { 00068 switch (Gscale) 00069 { 00070 // Possible gyro scales (and their register bit settings) are: 00071 // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11). 00072 // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: 00073 case GFS_250DPS: 00074 gRes = 250.0/32768.0; 00075 break; 00076 case GFS_500DPS: 00077 gRes = 500.0/32768.0; 00078 break; 00079 case GFS_1000DPS: 00080 gRes = 1000.0/32768.0; 00081 break; 00082 case GFS_2000DPS: 00083 gRes = 2000.0/32768.0; 00084 break; 00085 } 00086 } 00087 00088 00089 void MPU9250::getAres() { 00090 switch (Ascale) 00091 { 00092 // Possible accelerometer scales (and their register bit settings) are: 00093 // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11). 00094 // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: 00095 case AFS_2G: 00096 aRes = 2.0/32768.0; 00097 break; 00098 case AFS_4G: 00099 aRes = 4.0/32768.0; 00100 break; 00101 case AFS_8G: 00102 aRes = 8.0/32768.0; 00103 break; 00104 case AFS_16G: 00105 aRes = 16.0/32768.0; 00106 break; 00107 } 00108 } 00109 00110 00111 void MPU9250::readAccelData(int16_t * destination) 00112 { 00113 uint8_t rawData[6]; // x/y/z accel register data stored here 00114 readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array 00115 destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value 00116 destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; 00117 destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 00118 } 00119 00120 void MPU9250::readGyroData(int16_t * destination) 00121 { 00122 uint8_t rawData[6]; // x/y/z gyro register data stored here 00123 readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array 00124 destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value 00125 destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; 00126 destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 00127 } 00128 00129 void MPU9250::readMagData(int16_t * destination) 00130 { 00131 uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition 00132 if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set 00133 readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]); // Read the six raw data and ST2 registers sequentially into data array 00134 uint8_t c = rawData[6]; // End data read by reading ST2 register 00135 if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data 00136 destination[0] = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value 00137 destination[1] = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]) ; // Data stored as little Endian 00138 destination[2] = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]) ; 00139 } 00140 } 00141 } 00142 00143 int16_t MPU9250::readTempData() 00144 { 00145 uint8_t rawData[2]; // x/y/z gyro register data stored here 00146 readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array 00147 return (int16_t)(((int16_t)rawData[0]) << 8 | rawData[1]) ; // Turn the MSB and LSB into a 16-bit value 00148 } 00149 00150 00151 void MPU9250::resetMPU9250() { 00152 // reset device 00153 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device 00154 wait(0.1); 00155 } 00156 00157 void MPU9250::initAK8963(float * destination) 00158 { 00159 // First extract the factory calibration for each magnetometer axis 00160 uint8_t rawData[3]; // x/y/z gyro calibration data stored here 00161 writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer 00162 wait(0.01); 00163 writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode 00164 wait(0.01); 00165 readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values 00166 destination[0] = (float)(rawData[0] - 128)/256.0f + 1.0f; // Return x-axis sensitivity adjustment values, etc. 00167 destination[1] = (float)(rawData[1] - 128)/256.0f + 1.0f; 00168 destination[2] = (float)(rawData[2] - 128)/256.0f + 1.0f; 00169 writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer 00170 wait(0.01); 00171 // Configure the magnetometer for continuous read and highest resolution 00172 // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register, 00173 // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates 00174 writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR 00175 wait(0.01); 00176 } 00177 00178 00179 void MPU9250::initMPU9250() 00180 { 00181 // Initialize MPU9250 device 00182 // wake up device 00183 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors 00184 wait(0.1); // Delay 100 ms for PLL to get established on x-axis gyro; should check for PLL ready interrupt 00185 00186 // get stable time source 00187 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001 00188 00189 // Configure Gyro and Accelerometer 00190 // Disable FSYNC and set accelerometer and gyro bandwidth to 44 and 42 Hz, respectively; 00191 // DLPF_CFG = bits 2:0 = 010; this sets the sample rate at 1 kHz for both 00192 // Maximum delay is 4.9 ms which is just over a 200 Hz maximum rate 00193 writeByte(MPU9250_ADDRESS, CONFIG, 0x03); 00194 00195 // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV) 00196 writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; the same rate set in CONFIG above 00197 00198 // Set gyroscope full scale range 00199 // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3 00200 uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG); 00201 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0xE0); // Clear self-test bits [7:5] 00202 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0x18); // Clear AFS bits [4:3] 00203 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c | Gscale << 3); // Set full scale range for the gyro 00204 00205 // Set accelerometer configuration 00206 c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG); 00207 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0xE0); // Clear self-test bits [7:5] 00208 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0x18); // Clear AFS bits [4:3] 00209 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c | Ascale << 3); // Set full scale range for the accelerometer 00210 00211 // Set accelerometer sample rate configuration 00212 // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for 00213 // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz 00214 c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2); 00215 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c & ~0x0F); // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0]) 00216 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c | 0x03); // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz 00217 00218 // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates, 00219 // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting 00220 00221 // Configure Interrupts and Bypass Enable 00222 // Set interrupt pin active high, push-pull, and clear on read of INT_STATUS, enable I2C_BYPASS_EN so additional chips 00223 // can join the I2C bus and all can be controlled by the Arduino as master 00224 writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22); 00225 writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt 00226 } 00227 00228 // Function which accumulates gyro and accelerometer data after device initialization. It calculates the average 00229 // of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers. 00230 void MPU9250::calibrateMPU9250(float * dest1, float * dest2) 00231 { 00232 uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data 00233 uint16_t ii, packet_count, fifo_count; 00234 int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; 00235 00236 // reset device, reset all registers, clear gyro and accelerometer bias registers 00237 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device 00238 wait(0.1); 00239 00240 // get stable time source 00241 // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001 00242 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); 00243 writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00); 00244 wait(0.2); 00245 00246 // Configure device for bias calculation 00247 writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts 00248 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO 00249 writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source 00250 writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master 00251 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes 00252 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP 00253 wait(0.015); 00254 00255 // Configure MPU9250 gyro and accelerometer for bias calculation 00256 writeByte(MPU9250_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz 00257 writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz 00258 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity 00259 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity 00260 00261 uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec 00262 uint16_t accelsensitivity = 16384; // = 16384 LSB/g 00263 00264 // Configure FIFO to capture accelerometer and gyro data for bias calculation 00265 writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO 00266 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9250) 00267 wait(0.04); // accumulate 40 samples in 80 milliseconds = 480 bytes 00268 00269 // At end of sample accumulation, turn off FIFO sensor read 00270 writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO 00271 readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count 00272 fifo_count = ((uint16_t)data[0] << 8) | data[1]; 00273 packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging 00274 00275 for (ii = 0; ii < packet_count; ii++) { 00276 int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0}; 00277 readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging 00278 accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO 00279 accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ; 00280 accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ; 00281 gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ; 00282 gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ; 00283 gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ; 00284 00285 accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases 00286 accel_bias[1] += (int32_t) accel_temp[1]; 00287 accel_bias[2] += (int32_t) accel_temp[2]; 00288 gyro_bias[0] += (int32_t) gyro_temp[0]; 00289 gyro_bias[1] += (int32_t) gyro_temp[1]; 00290 gyro_bias[2] += (int32_t) gyro_temp[2]; 00291 00292 } 00293 accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases 00294 accel_bias[1] /= (int32_t) packet_count; 00295 accel_bias[2] /= (int32_t) packet_count; 00296 gyro_bias[0] /= (int32_t) packet_count; 00297 gyro_bias[1] /= (int32_t) packet_count; 00298 gyro_bias[2] /= (int32_t) packet_count; 00299 00300 if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation 00301 else {accel_bias[2] += (int32_t) accelsensitivity;} 00302 00303 // Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup 00304 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 00305 data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases 00306 data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF; 00307 data[3] = (-gyro_bias[1]/4) & 0xFF; 00308 data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF; 00309 data[5] = (-gyro_bias[2]/4) & 0xFF; 00310 00311 /// Push gyro biases to hardware registers 00312 /* writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]); 00313 writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]); 00314 writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]); 00315 writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]); 00316 writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]); 00317 writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]); 00318 */ 00319 dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; // construct gyro bias in deg/s for later manual subtraction 00320 dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity; 00321 dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity; 00322 00323 // Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain 00324 // factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold 00325 // non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature 00326 // compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that 00327 // the accelerometer biases calculated above must be divided by 8. 00328 00329 int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases 00330 readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values 00331 accel_bias_reg[0] = (int16_t) ((int16_t)data[0] << 8) | data[1]; 00332 readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]); 00333 accel_bias_reg[1] = (int16_t) ((int16_t)data[0] << 8) | data[1]; 00334 readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]); 00335 accel_bias_reg[2] = (int16_t) ((int16_t)data[0] << 8) | data[1]; 00336 00337 uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers 00338 uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis 00339 00340 for(ii = 0; ii < 3; ii++) { 00341 if(accel_bias_reg[ii] & mask) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit 00342 } 00343 00344 // Construct total accelerometer bias, including calculated average accelerometer bias from above 00345 accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale) 00346 accel_bias_reg[1] -= (accel_bias[1]/8); 00347 accel_bias_reg[2] -= (accel_bias[2]/8); 00348 00349 data[0] = (accel_bias_reg[0] >> 8) & 0xFF; 00350 data[1] = (accel_bias_reg[0]) & 0xFF; 00351 data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers 00352 data[2] = (accel_bias_reg[1] >> 8) & 0xFF; 00353 data[3] = (accel_bias_reg[1]) & 0xFF; 00354 data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers 00355 data[4] = (accel_bias_reg[2] >> 8) & 0xFF; 00356 data[5] = (accel_bias_reg[2]) & 0xFF; 00357 data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers 00358 00359 // Apparently this is not working for the acceleration biases in the MPU-9250 00360 // Are we handling the temperature correction bit properly? 00361 // Push accelerometer biases to hardware registers 00362 /* writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]); 00363 writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]); 00364 writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]); 00365 writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]); 00366 writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]); 00367 writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]); 00368 */ 00369 // Output scaled accelerometer biases for manual subtraction in the main program 00370 dest2[0] = (float)accel_bias[0]/(float)accelsensitivity; 00371 dest2[1] = (float)accel_bias[1]/(float)accelsensitivity; 00372 dest2[2] = (float)accel_bias[2]/(float)accelsensitivity; 00373 } 00374 00375 00376 // Accelerometer and gyroscope self test; check calibration wrt factory settings 00377 void MPU9250::MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass 00378 { 00379 uint8_t rawData[6] = {0, 0, 0, 0, 0, 0}; 00380 uint8_t selfTest[6]; 00381 int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3]; 00382 float factoryTrim[6]; 00383 uint8_t FS = 0; 00384 00385 writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz 00386 writeByte(MPU9250_ADDRESS, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz 00387 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1<<FS); // Set full scale range for the gyro to 250 dps 00388 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz 00389 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 1<<FS); // Set full scale range for the accelerometer to 2 g 00390 00391 for( int ii = 0; ii < 200; ii++) { // get average current values of gyro and acclerometer 00392 00393 readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array 00394 aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value 00395 aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; 00396 aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 00397 00398 readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array 00399 gAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value 00400 gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; 00401 gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 00402 } 00403 00404 for (int ii =0; ii < 3; ii++) { // Get average of 200 values and store as average current readings 00405 aAvg[ii] /= 200; 00406 gAvg[ii] /= 200; 00407 } 00408 00409 // Configure the accelerometer for self-test 00410 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0xE0); // Enable self test on all three axes and set accelerometer range to +/- 2 g 00411 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s 00412 wait_ms(25); // Delay a while to let the device stabilize 00413 00414 for( int ii = 0; ii < 200; ii++) { // get average self-test values of gyro and acclerometer 00415 00416 readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array 00417 aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value 00418 aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; 00419 aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 00420 00421 readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array 00422 gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value 00423 gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; 00424 gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 00425 } 00426 00427 for (int ii =0; ii < 3; ii++) { // Get average of 200 values and store as average self-test readings 00428 aSTAvg[ii] /= 200; 00429 gSTAvg[ii] /= 200; 00430 } 00431 00432 // Configure the gyro and accelerometer for normal operation 00433 writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); 00434 writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); 00435 wait_ms(25); // Delay a while to let the device stabilize 00436 00437 // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg 00438 selfTest[0] = readByte(MPU9250_ADDRESS, SELF_TEST_X_ACCEL); // X-axis accel self-test results 00439 selfTest[1] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_ACCEL); // Y-axis accel self-test results 00440 selfTest[2] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_ACCEL); // Z-axis accel self-test results 00441 selfTest[3] = readByte(MPU9250_ADDRESS, SELF_TEST_X_GYRO); // X-axis gyro self-test results 00442 selfTest[4] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_GYRO); // Y-axis gyro self-test results 00443 selfTest[5] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_GYRO); // Z-axis gyro self-test results 00444 00445 // Retrieve factory self-test value from self-test code reads 00446 factoryTrim[0] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[0] - 1.0) )); // FT[Xa] factory trim calculation 00447 factoryTrim[1] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[1] - 1.0) )); // FT[Ya] factory trim calculation 00448 factoryTrim[2] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[2] - 1.0) )); // FT[Za] factory trim calculation 00449 factoryTrim[3] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[3] - 1.0) )); // FT[Xg] factory trim calculation 00450 factoryTrim[4] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[4] - 1.0) )); // FT[Yg] factory trim calculation 00451 factoryTrim[5] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[5] - 1.0) )); // FT[Zg] factory trim calculation 00452 00453 // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response 00454 // To get percent, must multiply by 100 00455 for (int i = 0; i < 3; i++) { 00456 destination[i] = 100.0*((float)(aSTAvg[i] - aAvg[i]))/factoryTrim[i]; // Report percent differences 00457 destination[i+3] = 100.0*((float)(gSTAvg[i] - gAvg[i]))/factoryTrim[i+3]; // Report percent differences 00458 } 00459 00460 } 00461 00462 00463 00464 // Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays" 00465 // (see http://www.x-io.co.uk/category/open-source/ for examples and more details) 00466 // which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute 00467 // device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc. 00468 // The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms 00469 // but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz! 00470 void MPU9250::MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) 00471 { 00472 float GyroMeasError = PI * (60.0f / 180.0f); // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3 00473 float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta 00474 float GyroMeasDrift = PI * (1.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) 00475 float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value 00476 q[0] = 1.0f; 00477 q[1] = 0.0f; 00478 q[2] = 0.0f; 00479 q[3] = 0.0f; 00480 float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability 00481 float norm; 00482 float hx, hy, _2bx, _2bz; 00483 float s1, s2, s3, s4; 00484 float qDot1, qDot2, qDot3, qDot4; 00485 00486 // Auxiliary variables to avoid repeated arithmetic 00487 float _2q1mx; 00488 float _2q1my; 00489 float _2q1mz; 00490 float _2q2mx; 00491 float _4bx; 00492 float _4bz; 00493 float _2q1 = 2.0f * q1; 00494 float _2q2 = 2.0f * q2; 00495 float _2q3 = 2.0f * q3; 00496 float _2q4 = 2.0f * q4; 00497 float _2q1q3 = 2.0f * q1 * q3; 00498 float _2q3q4 = 2.0f * q3 * q4; 00499 float q1q1 = q1 * q1; 00500 float q1q2 = q1 * q2; 00501 float q1q3 = q1 * q3; 00502 float q1q4 = q1 * q4; 00503 float q2q2 = q2 * q2; 00504 float q2q3 = q2 * q3; 00505 float q2q4 = q2 * q4; 00506 float q3q3 = q3 * q3; 00507 float q3q4 = q3 * q4; 00508 float q4q4 = q4 * q4; 00509 00510 // Normalise accelerometer measurement 00511 norm = sqrt(ax * ax + ay * ay + az * az); 00512 if (norm == 0.0f) return; // handle NaN 00513 norm = 1.0f/norm; 00514 ax *= norm; 00515 ay *= norm; 00516 az *= norm; 00517 00518 // Normalise magnetometer measurement 00519 norm = sqrt(mx * mx + my * my + mz * mz); 00520 if (norm == 0.0f) return; // handle NaN 00521 norm = 1.0f/norm; 00522 mx *= norm; 00523 my *= norm; 00524 mz *= norm; 00525 00526 // Reference direction of Earth's magnetic field 00527 _2q1mx = 2.0f * q1 * mx; 00528 _2q1my = 2.0f * q1 * my; 00529 _2q1mz = 2.0f * q1 * mz; 00530 _2q2mx = 2.0f * q2 * mx; 00531 hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4; 00532 hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4; 00533 _2bx = sqrt(hx * hx + hy * hy); 00534 _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4; 00535 _4bx = 2.0f * _2bx; 00536 _4bz = 2.0f * _2bz; 00537 00538 // Gradient decent algorithm corrective step 00539 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); 00540 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); 00541 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); 00542 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); 00543 norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude 00544 norm = 1.0f/norm; 00545 s1 *= norm; 00546 s2 *= norm; 00547 s3 *= norm; 00548 s4 *= norm; 00549 00550 // Compute rate of change of quaternion 00551 qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1; 00552 qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2; 00553 qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3; 00554 qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4; 00555 00556 // Integrate to yield quaternion 00557 q1 += qDot1 * deltat; 00558 q2 += qDot2 * deltat; 00559 q3 += qDot3 * deltat; 00560 q4 += qDot4 * deltat; 00561 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion 00562 norm = 1.0f/norm; 00563 q[0] = q1 * norm; 00564 q[1] = q2 * norm; 00565 q[2] = q3 * norm; 00566 q[3] = q4 * norm; 00567 00568 } 00569 00570 00571 00572 // Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and 00573 // measured ones. 00574 void MPU9250::MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) 00575 { 00576 float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method 00577 q[0] = 1.0f; 00578 q[1] = 0.0f; 00579 q[2] = 0.0f; 00580 q[3] = 0.0f; 00581 float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability 00582 float norm; 00583 float hx, hy, bx, bz; 00584 float vx, vy, vz, wx, wy, wz; 00585 float ex, ey, ez; 00586 float pa, pb, pc; 00587 00588 // Auxiliary variables to avoid repeated arithmetic 00589 float q1q1 = q1 * q1; 00590 float q1q2 = q1 * q2; 00591 float q1q3 = q1 * q3; 00592 float q1q4 = q1 * q4; 00593 float q2q2 = q2 * q2; 00594 float q2q3 = q2 * q3; 00595 float q2q4 = q2 * q4; 00596 float q3q3 = q3 * q3; 00597 float q3q4 = q3 * q4; 00598 float q4q4 = q4 * q4; 00599 00600 // Normalise accelerometer measurement 00601 norm = sqrt(ax * ax + ay * ay + az * az); 00602 if (norm == 0.0f) return; // handle NaN 00603 norm = 1.0f / norm; // use reciprocal for division 00604 ax *= norm; 00605 ay *= norm; 00606 az *= norm; 00607 00608 // Normalise magnetometer measurement 00609 norm = sqrt(mx * mx + my * my + mz * mz); 00610 if (norm == 0.0f) return; // handle NaN 00611 norm = 1.0f / norm; // use reciprocal for division 00612 mx *= norm; 00613 my *= norm; 00614 mz *= norm; 00615 00616 // Reference direction of Earth's magnetic field 00617 hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3); 00618 hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2); 00619 bx = sqrt((hx * hx) + (hy * hy)); 00620 bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3); 00621 00622 // Estimated direction of gravity and magnetic field 00623 vx = 2.0f * (q2q4 - q1q3); 00624 vy = 2.0f * (q1q2 + q3q4); 00625 vz = q1q1 - q2q2 - q3q3 + q4q4; 00626 wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3); 00627 wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4); 00628 wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3); 00629 00630 // Error is cross product between estimated direction and measured direction of gravity 00631 ex = (ay * vz - az * vy) + (my * wz - mz * wy); 00632 ey = (az * vx - ax * vz) + (mz * wx - mx * wz); 00633 ez = (ax * vy - ay * vx) + (mx * wy - my * wx); 00634 if (Ki > 0.0f) 00635 { 00636 eInt[0] += ex; // accumulate integral error 00637 eInt[1] += ey; 00638 eInt[2] += ez; 00639 } 00640 else 00641 { 00642 eInt[0] = 0.0f; // prevent integral wind up 00643 eInt[1] = 0.0f; 00644 eInt[2] = 0.0f; 00645 } 00646 00647 // Apply feedback terms 00648 gx = gx + Kp * ex + Ki * eInt[0]; 00649 gy = gy + Kp * ey + Ki * eInt[1]; 00650 gz = gz + Kp * ez + Ki * eInt[2]; 00651 00652 // Integrate rate of change of quaternion 00653 pa = q2; 00654 pb = q3; 00655 pc = q4; 00656 q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat); 00657 q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat); 00658 q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat); 00659 q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat); 00660 00661 // Normalise quaternion 00662 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); 00663 norm = 1.0f / norm; 00664 q[0] = q1 * norm; 00665 q[1] = q2 * norm; 00666 q[2] = q3 * norm; 00667 q[3] = q4 * norm; 00668 00669 }
Generated on Sat Jul 23 2022 10:55:37 by
1.7.2