main.cpp

00001 /* MPU9150 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-9150 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  MPU9150 Breakout --------- Arduino
00017  VDD ---------------------- 3.3V
00018  VDDI --------------------- 3.3V
00019  SDA ----------------------- A4
00020  SCL ----------------------- A5
00021  GND ---------------------- GND
00022  
00023  Note: The MPU9150 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 "MPU9150.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, mcount = 0;
00045 char buffer[14];
00046 
00047    MPU9150 MPU9150;
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 = MPU9150.readByte(MPU9150_ADDRESS, WHO_AM_I_MPU9150);  // Read WHO_AM_I register for MPU-9250
00075   pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r");
00076   
00077   if (whoami == 0x68) // WHO_AM_I should be 0x68
00078   {  
00079     pc.printf("MPU9150 WHO_AM_I is 0x%x\n\r", whoami);
00080     pc.printf("MPU9150 is online...\n\r");
00081     lcd.clear();
00082     lcd.printString("MPU9150 is", 0, 0);
00083     sprintf(buffer, "0x%x", whoami);
00084     lcd.printString(buffer, 0, 1);
00085     lcd.printString("shoud be 0x68", 0, 2);  
00086     wait(1);
00087     
00088     MPU9150.MPU9150SelfTest(SelfTest);
00089     pc.printf("x-axis self test: acceleration trim within %f % of factory value\n\r", SelfTest[0]);
00090     pc.printf("y-axis self test: acceleration trim within %f % of factory value\n\r", SelfTest[1]);
00091     pc.printf("z-axis self test: acceleration trim within %f % of factory value\n\r", SelfTest[2]);
00092     pc.printf("x-axis self test: gyration trim within %f % of factory value\n\r", SelfTest[3]);
00093     pc.printf("y-axis self test: gyration trim within %f % of factory value\n\r", SelfTest[4]);
00094     pc.printf("z-axis self test: gyration trim within %f % of factory value\n\r", SelfTest[5]);
00095     wait(1);
00096     MPU9150.resetMPU9150(); // Reset registers to default in preparation for device calibration
00097     MPU9150.calibrateMPU9150(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
00098     pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
00099     pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
00100     pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
00101     pc.printf("x accel bias = %f\n\r", accelBias[0]);
00102     pc.printf("y accel bias = %f\n\r", accelBias[1]);
00103     pc.printf("z accel bias = %f\n\r", accelBias[2]);
00104     wait(1);
00105     MPU9150.initMPU9150(); 
00106     pc.printf("MPU9150 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
00107     MPU9150.magcalMPU9150(magbias);
00108     pc.printf("Mag cal done....\n\r"); // Initialize device for active mode read of magnetometer
00109     MPU9150.initAK8975A(magCalibration);
00110     pc.printf("AK8975 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
00111    }
00112    else
00113    {
00114     pc.printf("Could not connect to MPU9150: \n\r");
00115     pc.printf("%#x \n",  whoami);
00116  
00117     lcd.clear();
00118     lcd.printString("MPU9150", 0, 0);
00119     lcd.printString("no connection", 0, 1);
00120     sprintf(buffer, "WHO_AM_I 0x%x", whoami);
00121     lcd.printString(buffer, 0, 2); 
00122  
00123     while(1) ; // Loop forever if communication doesn't happen
00124     }
00125 
00126     uint8_t MagRate = 10; // set magnetometer read rate in Hz; 10 to 100 (max) Hz are reasonable values
00127     MPU9150.getAres(); // Get accelerometer sensitivity
00128     MPU9150.getGres(); // Get gyro sensitivity
00129     mRes = 10.*1229./4096.; // Conversion from 1229 microTesla full scale (4096) to 12.29 Gauss full scale
00130     // So far, magnetometer bias is calculated and subtracted here manually, should construct an algorithm to do it automatically
00131     // like the gyro and accelerometer biases
00132     /*
00133     magbias[0] = -5.;   // User environmental x-axis correction in milliGauss
00134     magbias[1] = -95.;  // User environmental y-axis correction in milliGauss
00135     magbias[2] = -260.; // User environmental z-axis correction in milliGauss
00136     */
00137 
00138  while(1) {
00139   
00140   // If intPin goes high, all data registers have new data
00141   if(MPU9150.readByte(MPU9150_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
00142 
00143     MPU9150.readAccelData(accelCount);  // Read the x/y/z adc values   
00144     // Now we'll calculate the accleration value into actual g's
00145     ax = (float)accelCount[0]*aRes; // - accelBias[0];  // get actual g value, this depends on scale being set
00146     ay = (float)accelCount[1]*aRes; // - accelBias[1];   
00147     az = (float)accelCount[2]*aRes; // - accelBias[2];  
00148    
00149     MPU9150.readGyroData(gyroCount);  // Read the x/y/z adc values
00150     // Calculate the gyro value into actual degrees per second
00151     gx = (float)gyroCount[0]*gRes; // - gyroBias[0];  // get actual gyro value, this depends on scale being set
00152     gy = (float)gyroCount[1]*gRes; // - gyroBias[1];  
00153     gz = (float)gyroCount[2]*gRes; // - gyroBias[2];   
00154   
00155     mcount++;
00156     if (mcount > 200/MagRate) {  // this is a poor man's way of setting the magnetometer read rate (see below) 
00157     MPU9150.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     mcount = 0;
00164     }
00165   }
00166    
00167     Now = t.read_us();
00168     deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
00169     lastUpdate = Now;
00170     
00171     sum += deltat;
00172     sumCount++;
00173     
00174 //    if(lastUpdate - firstUpdate > 10000000.0f) {
00175 //     beta = 0.04;  // decrease filter gain after stabilized
00176 //     zeta = 0.015; // increasey bias drift gain after stabilized
00177  //   }
00178     
00179    // Pass gyro rate as rad/s
00180 //  MPU9150.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
00181   MPU9150.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
00182 
00183     // Serial print and/or display at 0.5 s rate independent of data rates
00184     delt_t = t.read_ms() - count;
00185     if (delt_t > 500) { // update LCD once per half-second independent of read rate
00186 
00187     pc.printf("ax = %f", 1000*ax); 
00188     pc.printf(" ay = %f", 1000*ay); 
00189     pc.printf(" az = %f  mg\n\r", 1000*az); 
00190 
00191     pc.printf("gx = %f", gx); 
00192     pc.printf(" gy = %f", gy); 
00193     pc.printf(" gz = %f  deg/s\n\r", gz); 
00194     
00195     pc.printf("gx = %f", mx); 
00196     pc.printf(" gy = %f", my); 
00197     pc.printf(" gz = %f  mG\n\r", mz); 
00198     
00199     tempCount = MPU9150.readTempData();  // Read the adc values
00200     temperature = ((float) tempCount) / 340.0f + 36.53f; // Temperature in degrees Centigrade
00201     pc.printf(" temperature = %f  C\n\r", temperature); 
00202     
00203     pc.printf("q0 = %f\n\r", q[0]);
00204     pc.printf("q1 = %f\n\r", q[1]);
00205     pc.printf("q2 = %f\n\r", q[2]);
00206     pc.printf("q3 = %f\n\r", q[3]);      
00207     
00208 /*    lcd.clear();
00209     lcd.printString("MPU9150", 0, 0);
00210     lcd.printString("x   y   z", 0, 1);
00211     sprintf(buffer, "%d %d %d mg", (int)(1000.0f*ax), (int)(1000.0f*ay), (int)(1000.0f*az));
00212     lcd.printString(buffer, 0, 2);
00213     sprintf(buffer, "%d %d %d deg/s", (int)gx, (int)gy, (int)gz);
00214     lcd.printString(buffer, 0, 3);
00215     sprintf(buffer, "%d %d %d mG", (int)mx, (int)my, (int)mz);
00216     lcd.printString(buffer, 0, 4); 
00217  */  
00218   // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
00219   // In this coordinate system, the positive z-axis is down toward Earth. 
00220   // 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.
00221   // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
00222   // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
00223   // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
00224   // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
00225   // applied in the correct order which for this configuration is yaw, pitch, and then roll.
00226   // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
00227     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]);   
00228     pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
00229     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]);
00230     pitch *= 180.0f / PI;
00231     yaw   *= 180.0f / PI; 
00232     yaw   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
00233     roll  *= 180.0f / PI;
00234 
00235     pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
00236     pc.printf("average rate = %f\n\r", (float) sumCount/sum);
00237 //    sprintf(buffer, "YPR: %f %f %f", yaw, pitch, roll);
00238 //    lcd.printString(buffer, 0, 4);
00239 //    sprintf(buffer, "rate = %f", (float) sumCount/sum);
00240 //    lcd.printString(buffer, 0, 5);
00241     
00242     myled= !myled;
00243     count = t.read_ms(); 
00244 
00245     if(count > 1<<21) {
00246         t.start(); // start the timer over again if ~30 minutes has passed
00247         count = 0;
00248         deltat= 0;
00249         lastUpdate = t.read_us();
00250     }
00251     sum = 0;
00252     sumCount = 0; 
00253 }
00254 }
00255  
00256  }