Diff: main.cpp

Revision:

6:b272bd888e98

diff -r 7d1bf3ce0053 -r b272bd888e98 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jul 02 19:38:47 2014 +0000
@@ -0,0 +1,124 @@
+#include "mbed.h"
+#include "I2Cdev.h"
+#include "MPU6050_6Axis_MotionApps20.h"
+#include "shared.h"
+
+#ifndef M_PI
+#define M_PI 3.1415
+#endif
+
+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
+int count = 0;
+
+int main()
+{
+    pc.baud(115200);
+    pc.printf("MPU6050 initialize \r\n");
+
+    mpu.initialize();
+    pc.printf("MPU6050 testConnection \r\n");
+
+    if(mpu.testConnection())
+    {
+        pc.printf("MPU6050 test passed \r\n");
+    }
+    else
+    {
+        pc.printf("MPU6050 test failed \r\n");
+    }
+    
+    devStatus = mpu.dmpInitialize();
+ 
+    // supply your own gyro offsets here, scaled for min sensitivity
+    mpu.setXGyroOffset(-61);
+    mpu.setYGyroOffset(-127);
+    mpu.setZGyroOffset(19);
+    mpu.setZAccelOffset(16282); // 1688 factory default for my test chip
+ 
+    // make sure it worked (returns 0 if so)
+    if (devStatus == 0)
+    {
+        // turn on the DMP, now that it's ready
+        pc.printf("Enabling DMP...\r\n");
+        mpu.setDMPEnabled(true);
+ 
+        mpuIntStatus = mpu.getIntStatus();
+ 
+        // set our DMP Ready flag so the main loop() function knows it's okay to use it
+        pc.printf("DMP ready!\r\n");
+        dmpReady = true;
+ 
+        // get expected DMP packet size for later comparison
+        packetSize = mpu.dmpGetFIFOPacketSize();
+    }
+    else
+    {
+        // ERROR!
+        // 1 = initial memory load failed
+        // 2 = DMP configuration updates failed
+        // (if it's going to break, usually the code will be 1)
+        pc.printf("DMP Initialization failed (code %d )\r\n", devStatus);
+        while (1);
+    }
+    
+    while (1);
+    
+    while (1)
+    {
+        mpuIntStatus = mpu.getIntStatus();
+        fifoCount = mpu.getFIFOCount();
+        
+        // Check overflow
+        if ((mpuIntStatus & 0x10) || fifoCount == 1024)
+        {
+            // reset so we can continue cleanly
+            mpu.resetFIFO();
+            pc.printf("FIFO overflow!\r\n");
+        }
+        
+        if (mpuIntStatus & 0x02)
+        {
+            // wait for correct available data length, should be a VERY short wait
+            while (fifoCount < packetSize)
+            {
+                fifoCount = mpu.getFIFOCount();
+            }
+            
+            mpu.getFIFOBytes(fifoBuffer, packetSize);
+            
+            fifoCount -= packetSize;
+        }
+            
+        mpu.dmpGetQuaternion(&q, fifoBuffer);
+        mpu.dmpGetEuler(euler, &q);
+        
+        for (int i = 0; i < 3; i++)
+        {
+            euler[i] *= 180/M_PI;
+        }
+        
+        if (count++ > 20)
+        {
+            pc.printf("euler\t%d\t%d\t%d\r\n", euler[0], euler[1], euler[2]);
+            count = 0;
+        }
+    }
+
+}
\ No newline at end of file