main.cpp

00001 
00002 /* MPU6050 Basic Example Code
00003  by: Kris Winer
00004  date: May 1, 2014
00005  license: Beerware - Use this code however you'd like. If you 
00006  find it useful you can buy me a beer some time.
00007  
00008  Demonstrate  MPU-6050 basic functionality including initialization, accelerometer trimming, sleep mode functionality as well as
00009  parameterizing the register addresses. Added display functions to allow display to on breadboard monitor. 
00010  No DMP use. We just want to get out the accelerations, temperature, and gyro readings.
00011  
00012  SDA and SCL should have external pull-up resistors (to 3.3V).
00013  10k resistors worked for me. They should be on the breakout
00014  board.
00015  
00016  Hardware setup:
00017  MPU6050 Breakout --------- Arduino
00018  3.3V --------------------- 3.3V
00019  SDA ----------------------- A4
00020  SCL ----------------------- A5
00021  GND ---------------------- GND
00022  
00023   Note: The MPU6050 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 "mbed.h"
00030 #include "MPU6050.h"
00031 
00032 // Using NOKIA 5110 monochrome 84 x 48 pixel display
00033 // pin 9 - Serial clock out (SCLK)
00034 // pin 8 - Serial data out (DIN)
00035 // pin 7 - Data/Command select (D/C)
00036 // pin 5 - LCD chip select (CS)
00037 // pin 6 - LCD reset (RST)
00038 //Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6);
00039 
00040 float sum = 0;
00041 uint32_t sumCount = 0;
00042 
00043    MPU6050 mpu6050;
00044    
00045    Timer t;
00046 
00047    Serial pc(USBTX, USBRX); // tx, rx
00048 
00049    //        VCC,   SCE,  RST,  D/C,  MOSI,S CLK, LED
00050         
00051 int main()
00052 {
00053   pc.baud(9600);  
00054 
00055   //Set up I2C
00056   i2c.frequency(400000);  // use fast (400 kHz) I2C   
00057   t.start();        
00058   // Read the WHO_AM_I register, this is a good test of communication
00059   uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
00060   //pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r");
00061   
00062   if (whoami == 0x68) // WHO_AM_I should always be 0x68
00063   {  
00064     //pc.printf("MPU6050 is online...");
00065     wait(1);
00066     
00067     mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
00068     //pc.printf("x-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[0]); pc.printf("% of factory value \n\r");
00069     //pc.printf("y-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[1]); pc.printf("% of factory value \n\r");
00070     //pc.printf("z-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[2]); pc.printf("% of factory value \n\r");
00071     //pc.printf("x-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[3]); pc.printf("% of factory value \n\r");
00072     //pc.printf("y-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[4]); pc.printf("% of factory value \n\r");
00073     //pc.printf("z-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[5]); pc.printf("% of factory value \n\r");
00074     wait(1);
00075 
00076     if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) 
00077     {
00078     mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
00079     mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
00080     mpu6050.initMPU6050(); //pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
00081 
00082     wait(2);
00083        }
00084     else
00085     {
00086     //pc.printf("Device did not the pass self-test!\n\r");
00087  
00088       }
00089     }
00090     else
00091     {
00092     //pc.printf("Could not connect to MPU6050: \n\r");
00093     //pc.printf("%#x \n",  whoami);
00094     while(1) ; // Loop forever if communication doesn't happen
00095   }
00096 
00097 
00098 
00099  while(1) {
00100   
00101   // If data ready bit set, all data registers have new data
00102   if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
00103     mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
00104     mpu6050.getAres();
00105     
00106     // Now we'll calculate the accleration value into actual g's
00107     ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
00108     ay = (float)accelCount[1]*aRes - accelBias[1];   
00109     az = (float)accelCount[2]*aRes - accelBias[2];  
00110    
00111     mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
00112     mpu6050.getGres();
00113  
00114     // Calculate the gyro value into actual degrees per second
00115     gx = (float)gyroCount[0]*gRes; // - gyroBias[0];  // get actual gyro value, this depends on scale being set
00116     gy = (float)gyroCount[1]*gRes; // - gyroBias[1];  
00117     gz = (float)gyroCount[2]*gRes; // - gyroBias[2];   
00118 
00119     tempCount = mpu6050.readTempData();  // Read the x/y/z adc values
00120     temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
00121    }  
00122    
00123     Now = t.read_us();
00124     deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
00125     lastUpdate = Now;
00126     
00127     sum += deltat;
00128     sumCount++;
00129     
00130     if(lastUpdate - firstUpdate > 10000000.0f) {
00131      beta = 0.04;  // decrease filter gain after stabilized
00132      zeta = 0.015; // increasey bias drift gain after stabilized
00133     }
00134     
00135    // Pass gyro rate as rad/s
00136     mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
00137 
00138     // Serial print and/or display at 0.5 s rate independent of data rates
00139     delt_t = t.read_ms() - count;
00140     if (delt_t > 500) { // update LCD once per half-second independent of read rate
00141 
00142     //pc.printf("ax = %f", 1000*ax); 
00143     //pc.printf(" ay = %f", 1000*ay); 
00144     //pc.printf(" az = %f  mg\n\r", 1000*az); 
00145 
00146     //pc.printf("gx = %f", gx); 
00147     //pc.printf(" gy = %f", gy); 
00148     //pc.printf(" gz = %f  deg/s\n\r", gz); 
00149     
00150     //pc.printf(" temperature = %f  C\n\r", temperature); 
00151     
00152     //pc.printf("q0 = %f\n\r", q[0]);
00153     //pc.printf("q1 = %f\n\r", q[1]);
00154     //pc.printf("q2 = %f\n\r", q[2]);
00155     //pc.printf("q3 = %f\n\r", q[3]);      
00156     
00157   // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
00158   // In this coordinate system, the positive z-axis is down toward Earth. 
00159   // 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.
00160   // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
00161   // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
00162   // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
00163   // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
00164   // applied in the correct order which for this configuration is yaw, pitch, and then roll.
00165   // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
00166     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]);   
00167     pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
00168     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]);
00169     pitch *= 180.0f / PI;
00170     yaw   *= 180.0f / PI; 
00171     roll  *= 180.0f / PI;
00172 
00173 //    pc.printf("Yaw, Pitch, Roll: \n\r");
00174 //    pc.printf("%f", yaw);
00175 //    pc.printf(", ");
00176 //    pc.printf("%f", pitch);
00177 //    pc.printf(", ");
00178 //    pc.printf("%f\n\r", roll);
00179 //    pc.printf("average rate = "); pc.printf("%f", (sumCount/sum)); pc.printf(" Hz\n\r");
00180 
00181      pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
00182      //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
00183  
00184     myled= !myled;
00185     count = t.read_ms(); 
00186     sum = 0;
00187     sumCount = 0; 
00188 }
00189 }
00190  
00191  }