sensors/mpu9250.c
void MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
{
	uint8_t rawData[6] = {0, 0, 0, 0, 0, 0};
	uint8_t selfTest[6];
	uint16_t i,ii;
	int32_t gAvg[3] = {0}, aAvg[3] = {0}, aSTAvg[3] = {0}, gSTAvg[3] = {0};
	float factoryTrim[6];
	uint8_t FS = 0;

	writeByte( SMPLRT_DIV, 0x00);    // Set gyro sample rate to 1 kHz
	writeByte( CONFIG, 0x02);        // Set gyro sample rate to 1 kHz and DLPF to 92 Hz
	writeByte( GYRO_CONFIG, FS<<3);  // Set full scale range for the gyro to 250 dps
	writeByte( ACCEL_CONFIG2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
	writeByte( ACCEL_CONFIG, FS<<3); // Set full scale range for the accelerometer to 2 g

	for(ii = 0; ii < 200; ii++) {  // get average current values of gyro and acclerometer

		readByte( 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]) ;

		readByte( 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 (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( ACCEL_CONFIG, 0xE0); // Enable self test on all three axes and set accelerometer range to +/- 2 g
	writeByte( 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(ii = 0; ii < 200; ii++) {  // get average self-test values of gyro and acclerometer

		readByte( 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]) ;

		readByte( 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 (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( ACCEL_CONFIG, 0x00);
	writeByte( GYRO_CONFIG,  0x00);
	delay(25);  // Delay a while to let the device stabilize

	// Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
	readByte( SELF_TEST_X_ACCEL,1, &selfTest[0]); // X-axis accel self-test results
	readByte( SELF_TEST_Y_ACCEL,1, &selfTest[1]); // Y-axis accel self-test results
	readByte( SELF_TEST_Z_ACCEL,1, &selfTest[2]); // Z-axis accel self-test results
	readByte( SELF_TEST_X_GYRO,1, &selfTest[3]);  // X-axis gyro self-test results
	readByte( SELF_TEST_Y_GYRO,1, &selfTest[4]);  // Y-axis gyro self-test results
	readByte( SELF_TEST_Z_GYRO,1, &selfTest[5]);  // 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 percent, must multiply by 100
	for (i = 0; i < 3; i++) {
		destination[i]   = 100.0*((float)(aSTAvg[i] - aAvg[i]))/factoryTrim[i] - 100.;   // Report percent differences
		destination[i+3] = 100.0*((float)(gSTAvg[i] - gAvg[i]))/factoryTrim[i+3] - 100.; // Report percent differences
	}
}
// Accelerometer and gyroscope self test; check calibration wrt factory settings

Accelerometer and gyroscope self test; check calibration wrt factory settings
Accelerometer and gyroscope self test ; check calibration wrt factory settings
void MPU9250SelfTest(float * destination) 
{
	uint8_t rawData[6] = {0, 0, 0, 0, 0, 0};
	uint8_t selfTest[6];
	uint16_t i,ii;
	int32_t gAvg[3] = {0}, aAvg[3] = {0}, aSTAvg[3] = {0}, gSTAvg[3] = {0};
	float factoryTrim[6];
	uint8_t FS = 0;
	writeByte( SMPLRT_DIV, 0x00);    
	writeByte( CONFIG, 0x02);        
	writeByte( GYRO_CONFIG, FS<<3);  
	writeByte( ACCEL_CONFIG2, 0x02); 
	writeByte( ACCEL_CONFIG, FS<<3); 
	for(ii = 0; ii < 200; ii++) {  
		readByte( ACCEL_XOUT_H, 6, &rawData[0]);        
		aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  
		aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
		aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
		readByte( GYRO_XOUT_H, 6, &rawData[0]);       
		gAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  
		gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
		gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
	}
	for (ii =0; ii < 3; ii++) {  
		aAvg[ii] /= 200;
		gAvg[ii] /= 200;
	}
	writeByte( ACCEL_CONFIG, 0xE0); 
	writeByte( GYRO_CONFIG,  0xE0); 
	delay(25);  
	for(ii = 0; ii < 200; ii++) {  
		readByte( ACCEL_XOUT_H, 6, &rawData[0]);  
		aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  
		aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
		aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
		readByte( GYRO_XOUT_H, 6, &rawData[0]);  
		gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  
		gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
		gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
	}
	for (ii =0; ii < 3; ii++) {  
		aSTAvg[ii] /= 200;
		gSTAvg[ii] /= 200;
	}
	writeByte( ACCEL_CONFIG, 0x00);
	writeByte( GYRO_CONFIG,  0x00);
	delay(25);  
	readByte( SELF_TEST_X_ACCEL,1, &selfTest[0]); 
	readByte( SELF_TEST_Y_ACCEL,1, &selfTest[1]); 
	readByte( SELF_TEST_Z_ACCEL,1, &selfTest[2]); 
	readByte( SELF_TEST_X_GYRO,1, &selfTest[3]);  
	readByte( SELF_TEST_Y_GYRO,1, &selfTest[4]);  
	readByte( SELF_TEST_Z_GYRO,1, &selfTest[5]);  
	factoryTrim[0] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[0] - 1.0) )); 
	factoryTrim[1] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[1] - 1.0) )); 
	factoryTrim[2] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[2] - 1.0) )); 
	factoryTrim[3] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[3] - 1.0) )); 
	factoryTrim[4] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[4] - 1.0) )); 
	factoryTrim[5] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[5] - 1.0) )); 
	for (i = 0; i < 3; i++) {
		destination[i]   = 100.0*((float)(aSTAvg[i] - aAvg[i]))/factoryTrim[i] - 100.;   
		destination[i+3] = 100.0*((float)(gSTAvg[i] - gAvg[i]))/factoryTrim[i+3] - 100.; 
	}
}
void MPU9250SelfTest ( float * destination ) { uint8_t rawData [ 6 ] = { 0 , 0 , 0 , 0 , 0 , 0 } ; uint8_t selfTest [ 6 ] ; uint16_t i , ii ; int32_t gAvg [ 3 ] = { 0 } , aAvg [ 3 ] = { 0 } , aSTAvg [ 3 ] = { 0 } , gSTAvg [ 3 ] = { 0 } ; float factoryTrim [ 6 ] ; uint8_t FS = 0 ; writeByte ( SMPLRT_DIV , 0x00 ) ; writeByte ( CONFIG , 0x02 ) ; writeByte ( GYRO_CONFIG , FS << 3 ) ; writeByte ( ACCEL_CONFIG2 , 0x02 ) ; writeByte ( ACCEL_CONFIG , FS << 3 ) ; for ( ii = 0 ; ii < 200 ; ii ++ ) { readByte ( ACCEL_XOUT_H , 6 , & rawData [ 0 ] ) ; aAvg [ 0 ] += ( int16_t ) ( ( ( int16_t ) rawData [ 0 ] << 8 ) | rawData [ 1 ] ) ; aAvg [ 1 ] += ( int16_t ) ( ( ( int16_t ) rawData [ 2 ] << 8 ) | rawData [ 3 ] ) ; aAvg [ 2 ] += ( int16_t ) ( ( ( int16_t ) rawData [ 4 ] << 8 ) | rawData [ 5 ] ) ; readByte ( GYRO_XOUT_H , 6 , & rawData [ 0 ] ) ; gAvg [ 0 ] += ( int16_t ) ( ( ( int16_t ) rawData [ 0 ] << 8 ) | rawData [ 1 ] ) ; gAvg [ 1 ] += ( int16_t ) ( ( ( int16_t ) rawData [ 2 ] << 8 ) | rawData [ 3 ] ) ; gAvg [ 2 ] += ( int16_t ) ( ( ( int16_t ) rawData [ 4 ] << 8 ) | rawData [ 5 ] ) ; } for ( ii = 0 ; ii < 3 ; ii ++ ) { aAvg [ ii ] /= 200 ; gAvg [ ii ] /= 200 ; } writeByte ( ACCEL_CONFIG , 0xE0 ) ; writeByte ( GYRO_CONFIG , 0xE0 ) ; delay ( 25 ) ; for ( ii = 0 ; ii < 200 ; ii ++ ) { readByte ( ACCEL_XOUT_H , 6 , & rawData [ 0 ] ) ; aSTAvg [ 0 ] += ( int16_t ) ( ( ( int16_t ) rawData [ 0 ] << 8 ) | rawData [ 1 ] ) ; aSTAvg [ 1 ] += ( int16_t ) ( ( ( int16_t ) rawData [ 2 ] << 8 ) | rawData [ 3 ] ) ; aSTAvg [ 2 ] += ( int16_t ) ( ( ( int16_t ) rawData [ 4 ] << 8 ) | rawData [ 5 ] ) ; readByte ( GYRO_XOUT_H , 6 , & rawData [ 0 ] ) ; gSTAvg [ 0 ] += ( int16_t ) ( ( ( int16_t ) rawData [ 0 ] << 8 ) | rawData [ 1 ] ) ; gSTAvg [ 1 ] += ( int16_t ) ( ( ( int16_t ) rawData [ 2 ] << 8 ) | rawData [ 3 ] ) ; gSTAvg [ 2 ] += ( int16_t ) ( ( ( int16_t ) rawData [ 4 ] << 8 ) | rawData [ 5 ] ) ; } for ( ii = 0 ; ii < 3 ; ii ++ ) { aSTAvg [ ii ] /= 200 ; gSTAvg [ ii ] /= 200 ; } writeByte ( ACCEL_CONFIG , 0x00 ) ; writeByte ( GYRO_CONFIG , 0x00 ) ; delay ( 25 ) ; readByte ( SELF_TEST_X_ACCEL , 1 , & selfTest [ 0 ] ) ; readByte ( SELF_TEST_Y_ACCEL , 1 , & selfTest [ 1 ] ) ; readByte ( SELF_TEST_Z_ACCEL , 1 , & selfTest [ 2 ] ) ; readByte ( SELF_TEST_X_GYRO , 1 , & selfTest [ 3 ] ) ; readByte ( SELF_TEST_Y_GYRO , 1 , & selfTest [ 4 ] ) ; readByte ( SELF_TEST_Z_GYRO , 1 , & selfTest [ 5 ] ) ; factoryTrim [ 0 ] = ( float ) ( 2620 / 1 << FS ) * ( pow ( 1.01 , ( ( float ) selfTest [ 0 ] - 1.0 ) ) ) ; factoryTrim [ 1 ] = ( float ) ( 2620 / 1 << FS ) * ( pow ( 1.01 , ( ( float ) selfTest [ 1 ] - 1.0 ) ) ) ; factoryTrim [ 2 ] = ( float ) ( 2620 / 1 << FS ) * ( pow ( 1.01 , ( ( float ) selfTest [ 2 ] - 1.0 ) ) ) ; factoryTrim [ 3 ] = ( float ) ( 2620 / 1 << FS ) * ( pow ( 1.01 , ( ( float ) selfTest [ 3 ] - 1.0 ) ) ) ; factoryTrim [ 4 ] = ( float ) ( 2620 / 1 << FS ) * ( pow ( 1.01 , ( ( float ) selfTest [ 4 ] - 1.0 ) ) ) ; factoryTrim [ 5 ] = ( float ) ( 2620 / 1 << FS ) * ( pow ( 1.01 , ( ( float ) selfTest [ 5 ] - 1.0 ) ) ) ; for ( i = 0 ; i < 3 ; i ++ ) { destination [ i ] = 100.0 * ( ( float ) ( aSTAvg [ i ] - aAvg [ i ] ) ) / factoryTrim [ i ] - 100. ; destination [ i + 3 ] = 100.0 * ( ( float ) ( gSTAvg [ i ] - gAvg [ i ] ) ) / factoryTrim [ i + 3 ] - 100. ; } }
Accelerometer and gyroscope self test; check calibration wrt factory settings
Accelerometer and gyroscope self test ; check calibration wrt factory settings
// Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
 // Set gyro sample rate to 1 kHz
 // Set gyro sample rate to 1 kHz and DLPF to 92 Hz
 // Set full scale range for the gyro to 250 dps
 // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
 // Set full scale range for the accelerometer to 2 g
 // get average current values of gyro and acclerometer
 // Read the six raw data registers into data array
 // Turn the MSB and LSB into a signed 16-bit value
 // Read the six raw data registers sequentially into data array
 // Turn the MSB and LSB into a signed 16-bit value
 // Get average of 200 values and store as average current readings
 // Configure the accelerometer for self-test
 // Enable self test on all three axes and set accelerometer range to +/- 2 g
 // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
 // Delay a while to let the device stabilize
 // get average self-test values of gyro and acclerometer
 // Read the six raw data registers into data array
 // Turn the MSB and LSB into a signed 16-bit value
 // Read the six raw data registers sequentially into data array
 // Turn the MSB and LSB into a signed 16-bit value
 // Get average of 200 values and store as average self-test readings
 // Configure the gyro and accelerometer for normal operation
 // Delay a while to let the device stabilize
 // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
 // X-axis accel self-test results
 // Y-axis accel self-test results
 // Z-axis accel self-test results
 // X-axis gyro self-test results
 // Y-axis gyro self-test results
 // Z-axis gyro self-test results
 // Retrieve factory self-test value from self-test code reads
 // FT[Xa] factory trim calculation
 // FT[Ya] factory trim calculation
 // FT[Za] factory trim calculation
 // FT[Xg] factory trim calculation
 // FT[Yg] factory trim calculation
 // 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 percent, must multiply by 100
 // Report percent differences
 // Report percent differences
