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