RollPitch.h

00001 #include "JJ_MPU6050_DMP_6Axis.h"  // 与DMP工作库的修改版本（见注释内）typedef enum 
00002 
00003 #define I2C_SPEED 400000L //I2C速度
00004 
00005 MPU6050 mpu;
00006 bool dmpReady = false;
00007 uint16_t packetSize;
00008 //extern Serial mpc;
00009 extern DigitalOut myled;
00010 bool dmpSetup()
00011 {
00012 #if 0
00013     Wire.begin();
00014     TWSR = 0;
00015     TWBR = ((16000000L / I2C_SPEED) - 16) / 2;
00016     TWCR = 1 << TWEN;
00017 #endif
00018 
00019     mpu.initialize();
00020     if(mpu.dmpInitialize() == 0) {
00021         mpu.setDMPEnabled(true);
00022         dmpReady = true;
00023         packetSize = mpu.dmpGetFIFOPacketSize();
00024     } else {
00025         dmpReady = false;
00026     }
00027     //mpc.printf("packetSize = %d\r\n", packetSize);
00028     return dmpReady;
00029 }
00030 
00031 #define PI 3.1415926
00032 uint8_t dmpGetYPR(float *_ypr)
00033 {
00034     Quaternion q;
00035     VectorFloat gravity;
00036     uint8_t fifoBuffer[18];
00037     if(!dmpReady)
00038         return 0;
00039 
00040     //myled = !myled;
00041     if(mpu.getFIFOCount() == 1024) {
00042         myled = 1;
00043         mpu.resetFIFO();
00044         while(mpu.getFIFOCount() < packetSize);
00045         mpu.getFIFOBytes(fifoBuffer, packetSize);
00046         mpu.dmpGetQuaternion(&q, fifoBuffer);
00047         mpu.dmpGetGravity(&gravity, &q);
00048         mpu.dmpGetYawPitchRoll(_ypr, &q, &gravity);
00049         for(int i=0; i<3; i++) {
00050             _ypr[i] *= 180/PI;
00051         }
00052     } else {
00053         while(mpu.getFIFOCount() < packetSize);
00054         mpu.getFIFOBytes(fifoBuffer, packetSize);
00055         mpu.dmpGetQuaternion(&q, fifoBuffer);
00056         mpu.dmpGetGravity(&gravity, &q);
00057         mpu.dmpGetYawPitchRoll(_ypr, &q, &gravity);
00058         for(int i=0; i<3; i++) {
00059             _ypr[i] *= 180/PI;
00060         }
00061         myled = 0;
00062     }
00063     return 0;
00064 }