Diff: main.cpp

Revision:

6:6300d9561dfd

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Jul 01 16:56:00 2014 +0000
@@ -0,0 +1,123 @@
+#include "mbed.h"
+#include "I2Cdev.h"
+#include "MPU6050_6Axis_MotionApps20.h"
+#include "shared.h"
+
+MPU6050 mpu;
+
+// MPU control/status vars
+bool dmpReady = false; // set true if DMP init was successful
+uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
+uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
+uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
+uint16_t fifoCount; // count of all bytes currently in FIFO
+uint8_t fifoBuffer[64]; // FIFO storage buffer
+
+// orientation/motion vars
+Quaternion q; // [w, x, y, z] quaternion container
+VectorInt16 aa; // [x, y, z] accel sensor measurements
+VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
+VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
+VectorFloat gravity; // [x, y, z] gravity vector
+float euler[3]; // [psi, theta, phi] Euler angle container
+float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
+
+volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
+void dmpDataReady()
+{
+    mpuInterrupt = true;
+}
+
+#ifndef M_PI
+#define M_PI 3.1415
+#endif
+
+I2C i2c();
+
+int main()
+{
+   pc.baud(115200);
+    
+    mpu.initialize();
+
+    // verify connection
+    pc.printf("Testing device connections...\r\n");
+    
+    bool mpu6050TestResult = mpu.testConnection();
+    if(mpu6050TestResult)
+    {
+        pc.printf("MPU6050 test passed \r\n");
+    }
+    else
+    {
+        pc.printf("MPU6050 test failed \r\n");
+    }  
+    
+    // load and configure the DMP
+    pc.printf("Initializing DMP...\n");
+    devStatus = mpu.dmpInitialize();
+
+    // supply your own gyro offsets here, scaled for min sensitivity
+    mpu.setXGyroOffset(0);
+    mpu.setYGyroOffset(0);
+    mpu.setZGyroOffset(0);
+    mpu.setZAccelOffset(16282); // 1688 factory default for my test chip
+    
+    if (devStatus==0)
+    {
+        pc.printf("OK!\r\n");
+        mpu.setDMPEnabled(true);
+        
+        mpuIntStatus = mpu.getIntStatus();
+        
+        packetSize = mpu.dmpGetFIFOPacketSize();
+    }
+    else
+    {
+        pc.printf("Failed with code %d\r\n", devStatus);
+        while(1);
+    }
+    
+    // Main prog loop
+    while (1)
+    {
+        wait_us(500);
+        
+        mpuIntStatus = mpu.getIntStatus();
+    
+        // get current FIFO count
+        fifoCount = mpu.getFIFOCount();
+    
+        // check for overflow (this should never happen unless our code is too inefficient)
+        if ((mpuIntStatus & 0x10) || fifoCount == 1024)
+        {
+            // reset so we can continue cleanly
+            mpu.resetFIFO();
+            //pc.printf("FIFO overflow!");
+        }
+        else if (mpuIntStatus & 0x02)
+        {
+            // wait for correct available data length, should be a VERY short wait
+            while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
+    
+            // read a packet from FIFO
+            mpu.getFIFOBytes(fifoBuffer, packetSize);
+            
+            // track FIFO count here in case there is > 1 packet available
+            // (this lets us immediately read more without waiting for an interrupt)
+            fifoCount -= packetSize;   
+            
+            // display Euler angles in degrees
+            mpu.dmpGetQuaternion(&q, fifoBuffer);
+            mpu.dmpGetGravity(&gravity, &q);
+            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
+            /*pc.printf("ypr\t");
+            pc.printf("%f3.2",ypr[0] * 180/M_PI);
+            pc.printf("\t");
+            pc.printf("%f3.2",ypr[1] * 180/M_PI);
+            pc.printf("\t");
+            pc.printf("%f3.2\n",ypr[2] * 180/M_PI);*/
+            
+        }
+    }
+}
\ No newline at end of file