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