MyMPU6050.h

00001 // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
00002 // 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
00003 /* ============================================
00004 I2Cdev device library code is placed under the MIT license
00005 Copyright (c) 2012 Jeff Rowberg
00006 */
00007 
00008 #include "I2Cdev.h"
00009 #include "MPU6050_6Axis_MotionApps20.h"
00010 # define M_PI           3.14159265358979323846
00011 
00012 
00013 class MyMPU6050 {
00014     
00015     public:
00016     
00017         MyMPU6050(PinName i2cSda, PinName i2cScl, PinName interrupt) : mpu(i2cSda, i2cScl), checkpin(interrupt){
00018             resetTheta();
00019         }
00020         
00021         void setPC(Serial *pc){
00022             _pc = pc;
00023         }
00024         
00025         void loop() {
00026             // if programming failed, don't try to do anything
00027             if (!dmpReady) return;
00028             
00029 //            _pc->printf("dmp ready\n");
00030             
00031             // wait for MPU interrupt or extra packet(s) available
00032             if (!mpuInterrupt && fifoCount < packetSize) {
00033                 return;
00034                 // other program behavior stuff here
00035                 // .
00036                 // .
00037                 // .
00038                 // if you are really paranoid you can frequently test in between other
00039                 // stuff to see if mpuInterrupt is true, and if so, "break;" from the
00040                 // while() loop to immediately process the MPU data
00041                 // .
00042                 // .
00043                 // .
00044             }
00045             
00046 //            _pc->printf("mpu interrupt \n");
00047                                 
00048             // reset interrupt flag and get INT_STATUS byte
00049             mpuInterrupt = false;
00050             mpuIntStatus = mpu.getIntStatus();
00051         
00052             // get current FIFO count
00053             fifoCount = mpu.getFIFOCount();
00054         
00055             // check for overflow (this should never happen unless our code is too inefficient)
00056             if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
00057                 // reset so we can continue cleanly
00058                 mpu.resetFIFO();
00059 //                _pc->printf("FIFO overflow!\n");
00060         
00061             // otherwise, check for DMP data ready interrupt (this should happen frequently)
00062             } else if (mpuIntStatus & 0x02) {
00063                 // wait for correct available data length, should be a VERY short wait
00064                 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
00065         
00066                 // read a packet from FIFO
00067                 mpu.getFIFOBytes(fifoBuffer, packetSize);
00068                 
00069                 // track FIFO count here in case there is > 1 packet available
00070                 // (this lets us immediately read more without waiting for an interrupt)
00071                 fifoCount -= packetSize;
00072         
00073                 // display Euler angles in degrees
00074                 mpu.dmpGetQuaternion(&q, fifoBuffer);
00075                 mpu.dmpGetGravity(&gravity, &q);
00076                 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
00077                 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
00078                 float newTheta;
00079                 if (ypr[1]>0) newTheta = ypr[2];
00080                 else if (ypr[2]<0) newTheta = -M_PI-ypr[2];
00081                 else newTheta = M_PI-ypr[2];
00082                 
00083                 mpu.dmpGetGyro(gyroData, fifoBuffer);
00084                 float newDTheta = -gyroData[2]/180.0*M_PI;
00085                 
00086                 if (!thetaInitFlag && abs(newTheta-theta)>0.3) {
00087 //                    _pc->printf("IMU interrupt clash\n");
00088                     mpu.resetFIFO();
00089                 } else {
00090                     thetaInitFlag = false;
00091                     theta = newTheta;
00092                     dtheta = newDTheta;
00093 //                    _pc->printf("loop %f\n", theta);
00094                 }
00095                                 
00096                                 
00097                 
00098             }
00099         }
00100         
00101         void dmpDataReady() {
00102             mpuInterrupt = true;
00103         }
00104         
00105         float getTheta(){
00106 //            _pc->printf("%f\n", theta);
00107             return theta;
00108         }
00109         
00110         float getDTheta(){
00111             return dtheta;
00112         }
00113         
00114         void disable(){
00115             dmpReady = false;            
00116             mpuInterrupt = false;
00117             checkpin.rise(NULL);
00118         }
00119         
00120         void disableInterrupt(){
00121             mpuInterrupt = false;
00122             checkpin.rise(NULL);
00123         }
00124         
00125         void enable(){
00126             setup();
00127         }
00128         
00129         void enableInterrupt(){
00130             checkpin.rise(this, &MyMPU6050::dmpDataReady);
00131         }
00132 
00133     private:
00134     
00135         Serial *_pc;
00136     
00137         MPU6050 mpu;
00138         
00139         bool dmpReady;  // set true if DMP init was successful
00140         uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
00141         uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
00142         uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
00143         uint16_t fifoCount;     // count of all bytes currently in FIFO
00144         uint8_t fifoBuffer[64]; // FIFO storage buffer
00145         
00146         // orientation/motion vars
00147         Quaternion q;           // [w, x, y, z]         quaternion container
00148         VectorFloat gravity;    // [x, y, z]            gravity vector
00149         float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
00150         int16_t gyroData[3];
00151         volatile float theta;
00152         bool thetaInitFlag;
00153         volatile float dtheta;
00154     
00155         InterruptIn checkpin;
00156         volatile bool mpuInterrupt;
00157         
00158         
00159         void resetTheta(){
00160             theta = 0;
00161             dtheta = 0;
00162             thetaInitFlag = true;
00163         }
00164         
00165         void setup() {
00166             
00167             resetTheta();
00168             
00169             dmpReady = false;            
00170             mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
00171         
00172             // initialize device
00173 //            pc.printf("Initializing I2C devices...\r\n");
00174             mpu.initialize();
00175         
00176             // verify connection
00177 //            pc.printf("Testing device connections...\r\n");
00178             mpu.testConnection();//if() pc.printf("MPU6050 connection successful\r\n");
00179 //            else pc.printf("MPU6050 connection failed\r\n");
00180         
00181             // load and configure the DMP
00182 //            pc.printf("Initializing DMP...\r\n");
00183             devStatus = mpu.dmpInitialize();
00184             
00185             // make sure it worked (returns 0 if so)
00186             if (devStatus == 0) {
00187                 // turn on the DMP, now that it's ready
00188 //                pc.printf("Enabling DMP...\r\n");
00189                 mpu.setDMPEnabled(true);
00190         
00191                 // enable Arduino interrupt detection
00192 //                pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n");
00193                 checkpin.rise(this, &MyMPU6050::dmpDataReady);
00194                 
00195                 mpuIntStatus = mpu.getIntStatus();
00196         
00197                 // set our DMP Ready flag so the main loop() function knows it's okay to use it
00198 //                pc.printf("DMP ready! Waiting for first interrupt...\r\n");
00199                 dmpReady = true;
00200         
00201                 // get expected DMP packet size for later comparison
00202                 packetSize = mpu.dmpGetFIFOPacketSize();
00203             } else {
00204                 // ERROR!
00205                 // 1 = initial memory load failed
00206                 // 2 = DMP configuration updates failed
00207                 // (if it's going to break, usually the code will be 1)
00208                 
00209 //                pc.printf("DDMP Initialization failed (code ");
00210 //                pc.printf("%d", devStatus);
00211 //                pc.printf(")\r\n");
00212             }
00213         
00214         }
00215 };