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