![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
九軸の制御
Dependencies: mbed
Fork of MPU9250AHRS by
Diff: MPU9250.h
- Revision:
- 2:4e59a37182df
- Parent:
- 0:2e5e65a6fb30
- Child:
- 3:51217f251b8d
--- a/MPU9250.h Tue Aug 05 01:34:45 2014 +0000 +++ b/MPU9250.h Tue Aug 05 01:37:23 2014 +0000 @@ -592,44 +592,85 @@ // Accelerometer and gyroscope self test; check calibration wrt factory settings void MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass { - uint8_t rawData[4] = {0, 0, 0, 0}; + uint8_t rawData[6] = {0, 0, 0, 0, 0, 0}; uint8_t selfTest[6]; + int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3]; float factoryTrim[6]; + uint8_t FS = 0; - // Configure the accelerometer for self-test - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0xF0); // Enable self test on all three axes and set accelerometer range to +/- 8 g - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s - wait(0.25); // Delay a while to let the device execute the self-test - rawData[0] = readByte(MPU9250_ADDRESS, SELF_TEST_X_ACCEL); // X-axis self-test results - rawData[1] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_ACCEL); // Y-axis self-test results - rawData[2] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_ACCEL); // Z-axis self-test results - rawData[3] = readByte(MPU9250_ADDRESS, SELF_TEST_A); // Mixed-axis self-test results - // Extract the acceleration test results first - selfTest[0] = (rawData[0] >> 3) | (rawData[3] & 0x30) >> 4 ; // XA_TEST result is a five-bit unsigned integer - selfTest[1] = (rawData[1] >> 3) | (rawData[3] & 0x0C) >> 4 ; // YA_TEST result is a five-bit unsigned integer - selfTest[2] = (rawData[2] >> 3) | (rawData[3] & 0x03) >> 4 ; // ZA_TEST result is a five-bit unsigned integer - // Extract the gyration test results first - selfTest[3] = rawData[0] & 0x1F ; // XG_TEST result is a five-bit unsigned integer - selfTest[4] = rawData[1] & 0x1F ; // YG_TEST result is a five-bit unsigned integer - selfTest[5] = rawData[2] & 0x1F ; // ZG_TEST result is a five-bit unsigned integer - // Process results to allow final comparison with factory set values - factoryTrim[0] = (4096.0f*0.34f)*(pow( (0.92f/0.34f) , ((selfTest[0] - 1.0f)/30.0f))); // FT[Xa] factory trim calculation - factoryTrim[1] = (4096.0f*0.34f)*(pow( (0.92f/0.34f) , ((selfTest[1] - 1.0f)/30.0f))); // FT[Ya] factory trim calculation - factoryTrim[2] = (4096.0f*0.34f)*(pow( (0.92f/0.34f) , ((selfTest[2] - 1.0f)/30.0f))); // FT[Za] factory trim calculation - factoryTrim[3] = ( 25.0f*131.0f)*(pow( 1.046f , (selfTest[3] - 1.0f) )); // FT[Xg] factory trim calculation - factoryTrim[4] = (-25.0f*131.0f)*(pow( 1.046f , (selfTest[4] - 1.0f) )); // FT[Yg] factory trim calculation - factoryTrim[5] = ( 25.0f*131.0f)*(pow( 1.046f , (selfTest[5] - 1.0f) )); // FT[Zg] factory trim calculation + writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz + writeByte(MPU9250_ADDRESS, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz + writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1<<FS); // Set full scale range for the gyro to 250 dps + writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz + writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 1<<FS); // Set full scale range for the accelerometer to 2 g + + for( int ii = 0; ii < 200; ii++) { // get average current values of gyro and acclerometer + + readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array + aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value + aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; + aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; + + readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array + gAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value + gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; + gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; + } + + for (int ii =0; ii < 3; ii++) { // Get average of 200 values and store as average current readings + aAvg[ii] /= 200; + gAvg[ii] /= 200; + } + +// Configure the accelerometer for self-test + writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0xE0); // Enable self test on all three axes and set accelerometer range to +/- 2 g + writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s + delay(25); // Delay a while to let the device stabilize + + for( int ii = 0; ii < 200; ii++) { // get average self-test values of gyro and acclerometer + + readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array + aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value + aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; + aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; + + readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array + gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value + gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; + gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; + } + + for (int ii =0; ii < 3; ii++) { // Get average of 200 values and store as average self-test readings + aSTAvg[ii] /= 200; + gSTAvg[ii] /= 200; + } + + // Configure the gyro and accelerometer for normal operation + writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); + writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); + delay(25); // Delay a while to let the device stabilize - // Output self-test results and factory trim calculation if desired - // Serial.println(selfTest[0]); Serial.println(selfTest[1]); Serial.println(selfTest[2]); - // Serial.println(selfTest[3]); Serial.println(selfTest[4]); Serial.println(selfTest[5]); - // Serial.println(factoryTrim[0]); Serial.println(factoryTrim[1]); Serial.println(factoryTrim[2]); - // Serial.println(factoryTrim[3]); Serial.println(factoryTrim[4]); Serial.println(factoryTrim[5]); + // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg + selfTest[0] = readByte(MPU9250_ADDRESS, SELF_TEST_X_ACCEL); // X-axis accel self-test results + selfTest[1] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_ACCEL); // Y-axis accel self-test results + selfTest[2] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_ACCEL); // Z-axis accel self-test results + selfTest[3] = readByte(MPU9250_ADDRESS, SELF_TEST_X_GYRO); // X-axis gyro self-test results + selfTest[4] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_GYRO); // Y-axis gyro self-test results + selfTest[5] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_GYRO); // Z-axis gyro self-test results + // Retrieve factory self-test value from self-test code reads + factoryTrim[0] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[0] - 1.0) )); // FT[Xa] factory trim calculation + factoryTrim[1] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[1] - 1.0) )); // FT[Ya] factory trim calculation + factoryTrim[2] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[2] - 1.0) )); // FT[Za] factory trim calculation + factoryTrim[3] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[3] - 1.0) )); // FT[Xg] factory trim calculation + factoryTrim[4] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[4] - 1.0) )); // FT[Yg] factory trim calculation + factoryTrim[5] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[5] - 1.0) )); // FT[Zg] factory trim calculation + // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response - // To get to percent, must multiply by 100 and subtract result from 100 - for (int i = 0; i < 6; i++) { - destination[i] = 100.0f + 100.0f*(selfTest[i] - factoryTrim[i])/factoryTrim[i]; // Report percent differences + // To get percent, must multiply by 100 + for (int i = 0; i < 3; i++) { + destination[i] = 100.0*((float)(aSTAvg[i] - aAvg[i]))/factoryTrim[i]; // Report percent differences + destination[i+3] = 100.0*((float)(gSTAvg[i] - gAvg[i]))/factoryTrim[i+3]; // Report percent differences } }