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