Diff: MyMPU6050.h

Revision:

9:ac76b5b24b18

Parent:

8:96fa92188874

Child:

10:6167cc7e7d60

--- a/MyMPU6050.h	Fri Dec 04 07:10:43 2015 +0000
+++ b/MyMPU6050.h	Sat Dec 05 00:40:23 2015 +0000
@@ -9,11 +9,13 @@
 #include "MPU6050_6Axis_MotionApps20.h"
 # define M_PI           3.14159265358979323846
 
+
 class MyMPU6050 {
     
     public:
     
         MyMPU6050(PinName i2cSda, PinName i2cScl, PinName interrupt) : mpu(i2cSda, i2cScl), checkpin(interrupt){
+            resetTheta();
         }
         
         void setPC(Serial *pc){
@@ -69,15 +71,25 @@
                 mpu.dmpGetGravity(&gravity, &q);
                 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
                 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
-                float _theta;
-                if (ypr[1]>0) _theta = ypr[2];
-                else if (ypr[2]<0) _theta = -M_PI-ypr[2];
-                else _theta = M_PI-ypr[2];
-                theta = _theta;
+                float newTheta;
+                if (ypr[1]>0) newTheta = ypr[2];
+                else if (ypr[2]<0) newTheta = -M_PI-ypr[2];
+                else newTheta = M_PI-ypr[2];
+                
+                mpu.dmpGetGyro(gyroData, fifoBuffer);
+                float newDTheta = gyroData[2]/180.0*M_PI;
+                
+                if (!thetaInitFlag && abs(newTheta-theta)>0.3) {
+                    _pc->printf("IMU interrupt clash\n");
+                    mpu.resetFIFO();
+                } else {
+                    thetaInitFlag = false;
+                    theta = newTheta;
+                    dtheta = newDTheta;
+                }
                                 
-                mpu.dmpGetGyro(gyroData, fifoBuffer);
-                float _dtheta = gyroData[2]/180.0*M_PI;
-                dtheta = _dtheta;
+                                
+                
             }
         }
         
@@ -122,16 +134,22 @@
         float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
         int16_t gyroData[3];
         float theta;
+        bool thetaInitFlag;
         float dtheta;
     
         InterruptIn checkpin;
         volatile bool mpuInterrupt;
         
         
+        void resetTheta(){
+            theta = 0;
+            dtheta = 0;
+            thetaInitFlag = true;
+        }
+        
         void setup() {
             
-            theta = 0;
-            dtheta = 0;
+            resetTheta();
             
             dmpReady = false;            
             mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high