sfas
Dependents: Bracky-MPU6050-DMP
Fork of MPU6050 by
Diff: MyMPU6050.h
- Revision:
- 9:ac76b5b24b18
- Parent:
- 8:96fa92188874
- Child:
- 10:6167cc7e7d60
diff -r 96fa92188874 -r ac76b5b24b18 MyMPU6050.h --- 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