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 #include "MODSERIAL.h"
00034 #include "MACROS.h"
00035 //#include "N5110.h"
00036 
00037 // Using NOKIA 5110 monochrome 84 x 48 pixel display
00038 // pin 9 - Serial clock out (SCLK)
00039 // pin 8 - Serial data out (DIN)
00040 // pin 7 - Data/Command select (D/C)
00041 // pin 5 - LCD chip select (CS)
00042 // pin 6 - LCD reset (RST)
00043 //Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6);
00044 
00045 float sum = 0;
00046 uint32_t sumCount = 0;
00047 char buffer[14];
00048 
00049    MPU9250 mpu9250;
00050    
00051    Timer t;
00052 
00053    Serial pc(USBTX, USBRX); // tx, rx
00054 
00055    //        VCC,   SCE,  RST,  D/C,  MOSI,S CLK, LED
00056    //N5110 lcd(PA_8, PB_10, PA_9, PA_6, PA_7, PA_5, PC_7);
00057    
00058 
00059         
00060 int main()
00061 {
00062   pc.baud(115200);  
00063   char ch;
00064 
00065   //Set up I2C
00066   i2c.frequency(400000);  // use fast (400 kHz) I2C  
00067   
00068   pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);   
00069   
00070   t.start();        
00071   
00072   //lcd.init();
00073   //lcd.setBrightness(0.05);
00074   while(1){
00075     ch = pc.getc(); 
00076     pc.printf(" %c \r\n", ch);
00077   }
00078   
00079   
00080     
00081   // Read the WHO_AM_I register, this is a good test of communication
00082   uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
00083   pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x74\n\r");
00084   
00085   if (whoami == 0x74) // WHO_AM_I should always be 0x68
00086   {  
00087     pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
00088     pc.printf("MPU9250 is online...\n\r");
00089     //lcd.clear();
00090     //lcd.printString("MPU9250 is", 0, 0);
00091     sprintf(buffer, "0x%x", whoami);
00092     //lcd.printString(buffer, 0, 1);
00093     //lcd.printString("shoud be 0x71", 0, 2);  
00094     wait(1);
00095     
00096     mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
00097     mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
00098     pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);  
00099     pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);  
00100     pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);  
00101     pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);  
00102     pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);  
00103     pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);  
00104     mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
00105     pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
00106     pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
00107     pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
00108     pc.printf("x accel bias = %f\n\r", accelBias[0]);
00109     pc.printf("y accel bias = %f\n\r", accelBias[1]);
00110     pc.printf("z accel bias = %f\n\r", accelBias[2]);
00111     wait(2);
00112     mpu9250.initMPU9250(); 
00113     pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
00114     mpu9250.initAK8963(magCalibration);
00115     pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
00116     pc.printf("Accelerometer full-scale range = %f  g\n\r", 2.0f*(float)(1<<Ascale));
00117     pc.printf("Gyroscope full-scale range = %f  deg/s\n\r", 250.0f*(float)(1<<Gscale));
00118     if(Mscale == 0) pc.printf("Magnetometer resolution = 14  bits\n\r");
00119     if(Mscale == 1) pc.printf("Magnetometer resolution = 16  bits\n\r");
00120     if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
00121     if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
00122     wait(1);
00123    }
00124    else
00125    {
00126     pc.printf("Could not connect to MPU9250: \n\r");
00127     pc.printf("%#x \n",  whoami);
00128  
00129     //lcd.clear();
00130     //lcd.printString("MPU9250", 0, 0);
00131     //lcd.printString("no connection", 0, 1);
00132     sprintf(buffer, "WHO_AM_I 0x%x", whoami);
00133     //lcd.printString(buffer, 0, 2); 
00134  
00135     while(1) ; // Loop forever if communication doesn't happen
00136     }
00137 
00138     mpu9250.getAres(); // Get accelerometer sensitivity
00139     mpu9250.getGres(); // Get gyro sensitivity
00140     mpu9250.getMres(); // Get magnetometer sensitivity
00141     pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
00142     pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
00143     pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
00144     magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
00145     magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
00146     magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
00147 
00148  while(1) {
00149   
00150   // If intPin goes high, all data registers have new data
00151   if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
00152 
00153     mpu9250.readAccelData(accelCount);  // Read the x/y/z adc values   
00154     // Now we'll calculate the accleration value into actual g's
00155     ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
00156     ay = (float)accelCount[1]*aRes - accelBias[1];   
00157     az = (float)accelCount[2]*aRes - accelBias[2];  
00158    
00159     mpu9250.readGyroData(gyroCount);  // Read the x/y/z adc values
00160     // Calculate the gyro value into actual degrees per second
00161     gx = (float)gyroCount[0]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
00162     gy = (float)gyroCount[1]*gRes - gyroBias[1];  
00163     gz = (float)gyroCount[2]*gRes - gyroBias[2];   
00164   
00165     mpu9250.readMagData(magCount);  // Read the x/y/z adc values   
00166     // Calculate the magnetometer values in milliGauss
00167     // Include factory calibration per data sheet and user environmental corrections
00168     mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
00169     my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];  
00170     mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];   
00171   }
00172    
00173     Now = t.read_us();
00174     deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
00175     lastUpdate = Now;
00176     
00177     sum += deltat;
00178     sumCount++;
00179     
00180 //    if(lastUpdate - firstUpdate > 10000000.0f) {
00181 //     beta = 0.04;  // decrease filter gain after stabilized
00182 //     zeta = 0.015; // increasey bias drift gain after stabilized
00183  //   }
00184     
00185    // Pass gyro rate as rad/s
00186 //  mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
00187   mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
00188 
00189     // Serial print and/or display at 0.5 s rate independent of data rates
00190     delt_t = t.read_ms() - count;
00191     if (delt_t > 500) { // update LCD once per half-second independent of read rate
00192 
00193     pc.printf("ax = %f", 1000*ax); 
00194     pc.printf(" ay = %f", 1000*ay); 
00195     pc.printf(" az = %f  mg\n\r", 1000*az); 
00196 
00197     pc.printf("gx = %f", gx); 
00198     pc.printf(" gy = %f", gy); 
00199     pc.printf(" gz = %f  deg/s\n\r", gz); 
00200     
00201     pc.printf("gx = %f", mx); 
00202     pc.printf(" gy = %f", my); 
00203     pc.printf(" gz = %f  mG\n\r", mz); 
00204     
00205     tempCount = mpu9250.readTempData();  // Read the adc values
00206     temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
00207     pc.printf(" temperature = %f  C\n\r", temperature); 
00208     
00209     pc.printf("q0 = %f\n\r", q[0]);
00210     pc.printf("q1 = %f\n\r", q[1]);
00211     pc.printf("q2 = %f\n\r", q[2]);
00212     pc.printf("q3 = %f\n\r", q[3]);      
00213     
00214 /*    lcd.clear();
00215     lcd.printString("MPU9250", 0, 0);
00216     lcd.printString("x   y   z", 0, 1);
00217     sprintf(buffer, "%d %d %d mg", (int)(1000.0f*ax), (int)(1000.0f*ay), (int)(1000.0f*az));
00218     lcd.printString(buffer, 0, 2);
00219     sprintf(buffer, "%d %d %d deg/s", (int)gx, (int)gy, (int)gz);
00220     lcd.printString(buffer, 0, 3);
00221     sprintf(buffer, "%d %d %d mG", (int)mx, (int)my, (int)mz);
00222     lcd.printString(buffer, 0, 4); 
00223  */  
00224   // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
00225   // In this coordinate system, the positive z-axis is down toward Earth. 
00226   // 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.
00227   // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
00228   // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
00229   // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
00230   // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
00231   // applied in the correct order which for this configuration is yaw, pitch, and then roll.
00232   // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
00233     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]);   
00234     pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
00235     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]);
00236     pitch *= 180.0f / PI;
00237     yaw   *= 180.0f / PI; 
00238     yaw   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
00239     roll  *= 180.0f / PI;
00240 
00241     pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
00242     pc.printf("average rate = %f\n\r", (float) sumCount/sum);
00243 //    sprintf(buffer, "YPR: %f %f %f", yaw, pitch, roll);
00244 //    lcd.printString(buffer, 0, 4);
00245 //    sprintf(buffer, "rate = %f", (float) sumCount/sum);
00246 //    lcd.printString(buffer, 0, 5);
00247     
00248     myled= !myled;
00249     count = t.read_ms(); 
00250 
00251     if(count > 1<<21) {
00252         t.start(); // start the timer over again if ~30 minutes has passed
00253         count = 0;
00254         deltat= 0;
00255         lastUpdate = t.read_us();
00256     }
00257     sum = 0;
00258     sumCount = 0; 
00259 }
00260 }
00261  
00262  }