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