main.cpp

00001 #include "mbed.h"
00002 #include "I2Cdev.h"
00003 #include "MPU6050_6Axis_MotionApps20.h"
00004 #include "shared.h"
00005 
00006 MPU6050 mpu;
00007 
00008 // MPU control/status vars
00009 bool dmpReady = false; // set true if DMP init was successful
00010 uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
00011 uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
00012 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
00013 uint16_t fifoCount; // count of all bytes currently in FIFO
00014 uint8_t fifoBuffer[64]; // FIFO storage buffer
00015 
00016 // orientation/motion vars
00017 Quaternion q; // [w, x, y, z] quaternion container
00018 VectorInt16 aa; // [x, y, z] accel sensor measurements
00019 VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
00020 VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
00021 VectorFloat gravity; // [x, y, z] gravity vector
00022 float euler[3]; // [psi, theta, phi] Euler angle container
00023 float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
00024 
00025 volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
00026 void dmpDataReady()
00027 {
00028     mpuInterrupt = true;
00029 }
00030 
00031 #ifndef M_PI
00032 #define M_PI 3.1415
00033 #endif
00034 
00035 I2C i2c();
00036 
00037 int main()
00038 {
00039    pc.baud(115200);
00040     
00041     mpu.initialize();
00042 
00043     // verify connection
00044     pc.printf("Testing device connections...\r\n");
00045     
00046     bool mpu6050TestResult = mpu.testConnection();
00047     if(mpu6050TestResult)
00048     {
00049         pc.printf("MPU6050 test passed \r\n");
00050     }
00051     else
00052     {
00053         pc.printf("MPU6050 test failed \r\n");
00054     }  
00055     
00056     // load and configure the DMP
00057     pc.printf("Initializing DMP...\n");
00058     devStatus = mpu.dmpInitialize();
00059 
00060     // supply your own gyro offsets here, scaled for min sensitivity
00061     mpu.setXGyroOffset(0);
00062     mpu.setYGyroOffset(0);
00063     mpu.setZGyroOffset(0);
00064     mpu.setZAccelOffset(16282); // 1688 factory default for my test chip
00065     
00066     if (devStatus==0)
00067     {
00068         pc.printf("OK!\r\n");
00069         mpu.setDMPEnabled(true);
00070         
00071         mpuIntStatus = mpu.getIntStatus();
00072         
00073         packetSize = mpu.dmpGetFIFOPacketSize();
00074     }
00075     else
00076     {
00077         pc.printf("Failed with code %d\r\n", devStatus);
00078         while(1);
00079     }
00080     
00081     // Main prog loop
00082     while (1)
00083     {
00084         wait_us(500);
00085         
00086         mpuIntStatus = mpu.getIntStatus();
00087     
00088         // get current FIFO count
00089         fifoCount = mpu.getFIFOCount();
00090     
00091         // check for overflow (this should never happen unless our code is too inefficient)
00092         if ((mpuIntStatus & 0x10) || fifoCount == 1024)
00093         {
00094             // reset so we can continue cleanly
00095             mpu.resetFIFO();
00096             //pc.printf("FIFO overflow!");
00097         }
00098         else if (mpuIntStatus & 0x02)
00099         {
00100             // wait for correct available data length, should be a VERY short wait
00101             while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
00102     
00103             // read a packet from FIFO
00104             mpu.getFIFOBytes(fifoBuffer, packetSize);
00105             
00106             // track FIFO count here in case there is > 1 packet available
00107             // (this lets us immediately read more without waiting for an interrupt)
00108             fifoCount -= packetSize;   
00109             
00110             // display Euler angles in degrees
00111             mpu.dmpGetQuaternion(&q, fifoBuffer);
00112             mpu.dmpGetGravity(&gravity, &q);
00113             mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
00114             /*pc.printf("ypr\t");
00115             pc.printf("%f3.2",ypr[0] * 180/M_PI);
00116             pc.printf("\t");
00117             pc.printf("%f3.2",ypr[1] * 180/M_PI);
00118             pc.printf("\t");
00119             pc.printf("%f3.2\n",ypr[2] * 180/M_PI);*/
00120             
00121         }
00122     }
00123 }