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 "N5110.h"
00034 
00035 // Using NOKIA 5110 monochrome 84 x 48 pixel display
00036 // pin 9 - Serial clock out (SCLK)
00037 // pin 8 - Serial data out (DIN)
00038 // pin 7 - Data/Command select (D/C)
00039 // pin 5 - LCD chip select (CS)
00040 // pin 6 - LCD reset (RST)
00041 //Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6);
00042 
00043 float sum = 0;
00044 uint32_t sumCount = 0;
00045 int count = 0;
00046 float yaw = 0;
00047 float drift;
00048 MPU9250 imu(PTE25, PTE24);         // SDA, SCL 
00049    
00050    Timer t;
00051 
00052    Serial pc(USBTX, USBRX); // tx, rx
00053 
00054    //        VCC,   SCE,  RST,  D/C,  MOSI,S CLK, LED
00055   // N5110 lcd(PA_8, PB_10, PA_9, PA_6, PA_7, PA_5, PC_7);
00056    
00057 
00058         
00059 int main()
00060 {
00061   pc.baud(9600);  
00062   pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);   
00063     
00064   // Read the WHO_AM_I register, this is a good test of communication
00065   uint8_t whoami = imu.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
00066   pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
00067   
00068   if (whoami == 0x71) // WHO_AM_I should always be 0x68
00069   {  
00070     pc.printf("MPU9250 is online...\n\r");
00071     
00072     wait(1);
00073     
00074     imu.resetMPU9250(); // Reset registers to default in preparation for device calibration
00075     imu.calibrateMPU9250(imu.gyroBias, imu.accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
00076     imu.initMPU9250(); 
00077     imu.initAK8963(imu.magCalibration);
00078     wait(2);
00079    }
00080    else
00081    {
00082     pc.printf("Could not connect to MPU9250: \n\r");
00083     pc.printf("%#x \n",  whoami);
00084  
00085     while(1) ; // Loop forever if communication doesn't happen
00086     }
00087 
00088     imu.getAres(); // Get accelerometer sensitivity
00089     imu.getGres(); // Get gyro sensitivity
00090     imu.getMres(); // Get magnetometer sensitivity
00091     pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/imu.aRes);
00092     pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/imu.gRes);
00093     pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/imu.mRes);
00094     imu.magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
00095     imu.magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
00096     imu.magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
00097     t.start();        
00098 
00099  while(1) {
00100   
00101   // If intPin goes high, all data registers have new data
00102   if(imu.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
00103 
00104     imu.readAccelData(imu.accelCount);  // Read the x/y/z adc values   
00105     // Now we'll calculate the accleration value into actual g's
00106     imu.ax = (float)imu.accelCount[0]*imu.aRes - imu.accelBias[0];  // get actual g value, this depends on scale being set
00107     imu.ay = (float)imu.accelCount[1]*imu.aRes - imu.accelBias[1];   
00108     imu.az = (float)imu.accelCount[2]*imu.aRes - imu.accelBias[2];  
00109    
00110     imu.readGyroData(imu.gyroCount);  // Read the x/y/z adc values
00111     // Calculate the gyro value into actual degrees per second
00112     imu.gx = (float)imu.gyroCount[0]*imu.gRes - imu.gyroBias[0];  // get actual gyro value, this depends on scale being set
00113     imu.gy = (float)imu.gyroCount[1]*imu.gRes - imu.gyroBias[1];  
00114     imu.gz = (float)imu.gyroCount[2]*imu.gRes - imu.gyroBias[2];   
00115   
00116     imu.readMagData(imu.magCount);  // Read the x/y/z adc values   
00117     // Calculate the magnetometer values in milliGauss
00118     // Include factory calibration per data sheet and user environmental corrections
00119     imu.mx = (float)imu.magCount[0]*imu.mRes*imu.magCalibration[0] - imu.magbias[0];  // get actual magnetometer value, this depends on scale being set
00120     imu.my = (float)imu.magCount[1]*imu.mRes*imu.magCalibration[1] - imu.magbias[1];  
00121     imu.mz = (float)imu.magCount[2]*imu.mRes*imu.magCalibration[2] - imu.magbias[2];   
00122   }
00123    
00124   if(imu.gz>.3 || imu.gz < -.3){
00125      yaw = (yaw - t.read()*imu.gz+drift);
00126     t.reset();        
00127     if(yaw > 360)
00128         yaw -= 360;
00129     if(yaw < 0)
00130         yaw += 360;
00131     pc.printf("Yaw: %f \n\r", yaw);
00132     }
00133 
00134 }
00135 }
00136