main.cpp

00001 /* MPU9250 Basic Example Code
00002  by: Kris Winer
00003  date: April 1, 2014
00004  license: Beerware - Use this code however you'd like. If you 
00005  find it useful you can buy me a beer some time.
00006  
00007  Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor, 
00008  getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to 
00009  allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and 
00010  Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.
00011  
00012  SDA and SCL should have external pull-up resistors (to 3.3V).
00013  10k resistors are on the EMSENSR-9250 breakout board.
00014  
00015  Hardware setup:
00016  MPU9250 Breakout --------- Arduino
00017  VDD ---------------------- 3.3V
00018  VDDI --------------------- 3.3V
00019  SDA ----------------------- A4
00020  SCL ----------------------- A5
00021  GND ---------------------- GND
00022  
00023  Note: The MPU9250 is an I2C sensor and uses the Arduino Wire library. 
00024  Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1.
00025  We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file.
00026  We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ  to 400000L /twi.h utility file.
00027  */
00028  
00029 //#include "ST_F401_84MHZ.h" 
00030 //F401_init84 myinit(0);
00031 #include "mbed.h"
00032 #include "MPU9250.h"
00033 
00034 // Using NOKIA 5110 monochrome 84 x 48 pixel display
00035 // pin 9 - Serial clock out (SCLK)
00036 // pin 8 - Serial data out (DIN)
00037 // pin 7 - Data/Command select (D/C)
00038 // pin 5 - LCD chip select (CS)
00039 // pin 6 - LCD reset (RST)
00040 //Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6);
00041 
00042 float sum = 0;
00043 uint32_t sumCount = 0;
00044 char buffer[14];
00045 
00046    MPU9250 mpu9250;
00047    
00048    Timer t;
00049 
00050    Serial pc(USBTX, USBRX); // tx, rx
00051 
00052    //        VCC,   SCE,  RST,  D/C,  MOSI,S CLK, LED
00053 
00054    
00055 
00056         
00057 int main()
00058 {
00059   pc.baud(9600);  
00060 
00061   //Set up I2C
00062   i2c.frequency(400000);  // use fast (400 kHz) I2C  
00063   
00064   pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);   
00065   
00066   t.start();        
00067   
00068 
00069   
00070     
00071   // Read the WHO_AM_I register, this is a good test of communication
00072   uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
00073   pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
00074   
00075   if (whoami == 0x71) // WHO_AM_I should always be 0x68
00076   {  
00077     pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
00078     pc.printf("MPU9250 is online...\n\r");
00079 
00080     sprintf(buffer, "0x%x", whoami);
00081  
00082     wait(1);
00083     
00084     mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
00085     mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
00086     pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);  
00087     pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);  
00088     pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);  
00089     pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);  
00090     pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);  
00091     pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);  
00092     mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
00093     pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
00094     pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
00095     pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
00096     pc.printf("x accel bias = %f\n\r", accelBias[0]);
00097     pc.printf("y accel bias = %f\n\r", accelBias[1]);
00098     pc.printf("z accel bias = %f\n\r", accelBias[2]);
00099     wait(2);
00100     mpu9250.initMPU9250(); 
00101     pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
00102     mpu9250.initAK8963(magCalibration);
00103     pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
00104     pc.printf("Accelerometer full-scale range = %f  g\n\r", 2.0f*(float)(1<<Ascale));
00105     pc.printf("Gyroscope full-scale range = %f  deg/s\n\r", 250.0f*(float)(1<<Gscale));
00106     if(Mscale == 0) pc.printf("Magnetometer resolution = 14  bits\n\r");
00107     if(Mscale == 1) pc.printf("Magnetometer resolution = 16  bits\n\r");
00108     if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
00109     if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
00110     wait(1);
00111    }
00112    else
00113    {
00114     pc.printf("Could not connect to MPU9250: \n\r");
00115     pc.printf("%#x \n",  whoami);
00116  
00117 
00118     sprintf(buffer, "WHO_AM_I 0x%x", whoami);
00119 
00120  
00121     while(1) ; // Loop forever if communication doesn't happen
00122     }
00123 
00124     mpu9250.getAres(); // Get accelerometer sensitivity
00125     mpu9250.getGres(); // Get gyro sensitivity
00126     mpu9250.getMres(); // Get magnetometer sensitivity
00127     pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
00128     pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
00129     pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
00130     magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
00131     magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
00132     magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
00133 
00134  while(1) {
00135   
00136   // If intPin goes high, all data registers have new data
00137   if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
00138 
00139     mpu9250.readAccelData(accelCount);  // Read the x/y/z adc values   
00140     // Now we'll calculate the accleration value into actual g's
00141     ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
00142     ay = (float)accelCount[1]*aRes - accelBias[1];   
00143     az = (float)accelCount[2]*aRes - accelBias[2];  
00144    
00145     mpu9250.readGyroData(gyroCount);  // Read the x/y/z adc values
00146     // Calculate the gyro value into actual degrees per second
00147     gx = (float)gyroCount[0]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
00148     gy = (float)gyroCount[1]*gRes - gyroBias[1];  
00149     gz = (float)gyroCount[2]*gRes - gyroBias[2];   
00150   
00151     mpu9250.readMagData(magCount);  // Read the x/y/z adc values   
00152     // Calculate the magnetometer values in milliGauss
00153     // Include factory calibration per data sheet and user environmental corrections
00154     mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
00155     my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];  
00156     mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];   
00157   }
00158    
00159     Now = t.read_us();
00160     deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
00161     lastUpdate = Now;
00162     
00163     sum += deltat;
00164     sumCount++;
00165     
00166 //    if(lastUpdate - firstUpdate > 10000000.0f) {
00167 //     beta = 0.04;  // decrease filter gain after stabilized
00168 //     zeta = 0.015; // increasey bias drift gain after stabilized
00169  //   }
00170     
00171    // Pass gyro rate as rad/s
00172 //  mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
00173   mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
00174 
00175     // Serial print and/or display at 0.5 s rate independent of data rates
00176     delt_t = t.read_ms() - count;
00177     if (delt_t > 500) { // update LCD once per half-second independent of read rate
00178 
00179 //    pc.printf("ax = %f", 1000*ax); 
00180 //    pc.printf(" ay = %f", 1000*ay); 
00181 //    pc.printf(" az = %f  mg\n\r", 1000*az); 
00182 
00183     pc.printf("gx = %f", gx); 
00184     pc.printf(" gy = %f", gy); 
00185     pc.printf(" gz = %f  deg/s\n\r", gz); 
00186     
00187 //    pc.printf("gx = %f", mx); 
00188 //    pc.printf(" gy = %f", my); 
00189 //    pc.printf(" gz = %f  mG\n\r", mz); 
00190     
00191     tempCount = mpu9250.readTempData();  // Read the adc values
00192     temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
00193     //pc.printf(" temperature = %f  C\n\r", temperature); 
00194 //    
00195 //    pc.printf("q0 = %f\n\r", q[0]);
00196 //    pc.printf("q1 = %f\n\r", q[1]);
00197 //    pc.printf("q2 = %f\n\r", q[2]);
00198 //    pc.printf("q3 = %f\n\r", q[3]);      
00199     
00200 /*    lcd.clear();
00201     lcd.printString("MPU9250", 0, 0);
00202     lcd.printString("x   y   z", 0, 1);
00203     sprintf(buffer, "%d %d %d mg", (int)(1000.0f*ax), (int)(1000.0f*ay), (int)(1000.0f*az));
00204     lcd.printString(buffer, 0, 2);
00205     sprintf(buffer, "%d %d %d deg/s", (int)gx, (int)gy, (int)gz);
00206     lcd.printString(buffer, 0, 3);
00207     sprintf(buffer, "%d %d %d mG", (int)mx, (int)my, (int)mz);
00208     lcd.printString(buffer, 0, 4); 
00209  */  
00210   // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
00211   // In this coordinate system, the positive z-axis is down toward Earth. 
00212   // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
00213   // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
00214   // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
00215   // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
00216   // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
00217   // applied in the correct order which for this configuration is yaw, pitch, and then roll.
00218   // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
00219     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]);   
00220     pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
00221     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]);
00222     pitch *= 180.0f / PI;
00223     yaw   *= 180.0f / PI; 
00224     yaw   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
00225     roll  *= 180.0f / PI;
00226 
00227  //   pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
00228 //    pc.printf("average rate = %f\n\r", (float) sumCount/sum);
00229 //    sprintf(buffer, "YPR: %f %f %f", yaw, pitch, roll);
00230 //    lcd.printString(buffer, 0, 4);
00231 //    sprintf(buffer, "rate = %f", (float) sumCount/sum);
00232 //    lcd.printString(buffer, 0, 5);
00233     
00234     myled= !myled;
00235     count = t.read_ms(); 
00236 
00237     if(count > 1<<21) {
00238         t.start(); // start the timer over again if ~30 minutes has passed
00239         count = 0;
00240         deltat= 0;
00241         lastUpdate = t.read_us();
00242     }
00243     sum = 0;
00244     sumCount = 0; 
00245 }
00246 }
00247  
00248  }