Diff: RollPitch.h

Revision:

0:a4d8f5b3c546

Child:

1:620da20b810b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RollPitch.h	Sat Jun 04 03:16:52 2016 +0000
@@ -0,0 +1,50 @@
+#include <JJ_MPU6050_DMP_6Axis.h>  // 与DMP工作库的修改版本（见注释内）typedef enum 
+
+#define I2C_SPEED 400000L //I2C速度
+
+MPU6050 mpu;
+bool dmpReady = false;
+uint16_t packetSize;
+
+bool dmpSetup()
+{
+#if 0
+    Wire.begin();
+    TWSR = 0;
+    TWBR = ((16000000L / I2C_SPEED) - 16) / 2;
+    TWCR = 1 << TWEN;
+#endif
+
+    mpu.initialize();
+    if(mpu.dmpInitialize() == 0) {
+        mpu.setDMPEnabled(true);
+        dmpReady = true;
+        packetSize = mpu.dmpGetFIFOPacketSize();
+    } else {
+        dmpReady = false;
+    }
+    return dmpReady;
+}
+#define PI 3.1415926
+uint8_t dmpGetYPR(float *_ypr)
+{
+    Quaternion q;
+    VectorFloat gravity;
+    uint8_t fifoBuffer[18];
+    if(!dmpReady)
+        return 0;
+
+    if(mpu.getFIFOCount() == 1024) {
+        mpu.resetFIFO();
+    } else {
+        while(mpu.getFIFOCount() < packetSize);
+        mpu.getFIFOBytes(fifoBuffer, packetSize);
+        mpu.dmpGetQuaternion(&q, fifoBuffer);
+        mpu.dmpGetGravity(&gravity, &q);
+        mpu.dmpGetYawPitchRoll(_ypr, &q, &gravity);
+        for(int i=0; i<3; i++) {
+            _ypr[i] *= 180/PI;
+        }
+    }
+    return 0;
+}