Diff: main.cpp

Revision:

6:0bb3c17bdaf1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jun 25 19:53:14 2014 +0000
@@ -0,0 +1,89 @@
+#include "mbed.h"
+#include "I2Cdev.h"
+#include "helper_3dmath.h"
+#include "MPU6050_6Axis_MotionApps20.h"
+#include "MPU6050.h"
+
+
+MPU6050 mpu;
+Serial serOut(D8, D2);
+
+// 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;
+
+volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
+void dmpDataReady() {
+    mpuInterrupt = true;
+}
+
+int main()
+{
+    serOut.baud(115200);
+    /*
+    // Join the bus here
+    
+    // initialize device
+    serOut.printf("Initializing I2C devices...\r\n");
+    mpu.initialize();
+
+    // verify connection
+    serOut.printf("Testing device connections...\r\n");
+    serOut.printf(mpu.testConnection() ? "MPU6050 connection successful\r\n" : "MPU6050 connection failed\r\n");
+
+    // load and configure the DMP
+    serOut.printf("Initializing DMP...\r\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(0); // 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
+        serOut.printf("Enabling DMP...\r\n");
+        mpu.setDMPEnabled(true);
+
+        // enable Arduino interrupt detection
+        serOut.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n");
+        //attachInterrupt(0, dmpDataReady, RISING);
+        mpuIntStatus = mpu.getIntStatus();
+
+        // set our DMP Ready flag so the main loop() function knows it's okay to use it
+        serOut.printf("DMP ready! Waiting for first interrupt...\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)
+        serOut.printf("DMP Initialization failed (code \r\n");
+        serOut.printf("%d", devStatus);
+        serOut.printf(")\r\n");
+    }
+    */
+}
\ No newline at end of file