sfas
Dependents: Bracky-MPU6050-DMP
Fork of MPU6050 by
MyMPU6050.h@12:775e1464e042, 2015-12-09 (annotated)
- Committer:
- amandaghassaei
- Date:
- Wed Dec 09 03:25:10 2015 +0000
- Revision:
- 12:775e1464e042
- Parent:
- 11:feab67f916fe
enable interrupt
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
amandaghassaei | 6:748bae310e1e | 1 | // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0) |
amandaghassaei | 6:748bae310e1e | 2 | // 6/21/2012 by Jeff Rowberg <jeff@rowberg.net> |
amandaghassaei | 6:748bae310e1e | 3 | /* ============================================ |
amandaghassaei | 6:748bae310e1e | 4 | I2Cdev device library code is placed under the MIT license |
amandaghassaei | 6:748bae310e1e | 5 | Copyright (c) 2012 Jeff Rowberg |
amandaghassaei | 6:748bae310e1e | 6 | */ |
amandaghassaei | 6:748bae310e1e | 7 | |
amandaghassaei | 6:748bae310e1e | 8 | #include "I2Cdev.h" |
amandaghassaei | 6:748bae310e1e | 9 | #include "MPU6050_6Axis_MotionApps20.h" |
amandaghassaei | 7:07f1f975429a | 10 | # define M_PI 3.14159265358979323846 |
amandaghassaei | 6:748bae310e1e | 11 | |
amandaghassaei | 9:ac76b5b24b18 | 12 | |
amandaghassaei | 7:07f1f975429a | 13 | class MyMPU6050 { |
amandaghassaei | 6:748bae310e1e | 14 | |
amandaghassaei | 6:748bae310e1e | 15 | public: |
amandaghassaei | 6:748bae310e1e | 16 | |
amandaghassaei | 7:07f1f975429a | 17 | MyMPU6050(PinName i2cSda, PinName i2cScl, PinName interrupt) : mpu(i2cSda, i2cScl), checkpin(interrupt){ |
amandaghassaei | 9:ac76b5b24b18 | 18 | resetTheta(); |
amandaghassaei | 8:96fa92188874 | 19 | } |
amandaghassaei | 8:96fa92188874 | 20 | |
amandaghassaei | 8:96fa92188874 | 21 | void setPC(Serial *pc){ |
amandaghassaei | 8:96fa92188874 | 22 | _pc = pc; |
amandaghassaei | 6:748bae310e1e | 23 | } |
amandaghassaei | 6:748bae310e1e | 24 | |
amandaghassaei | 6:748bae310e1e | 25 | void loop() { |
amandaghassaei | 6:748bae310e1e | 26 | // if programming failed, don't try to do anything |
amandaghassaei | 6:748bae310e1e | 27 | if (!dmpReady) return; |
amandaghassaei | 10:6167cc7e7d60 | 28 | |
amandaghassaei | 10:6167cc7e7d60 | 29 | // _pc->printf("dmp ready\n"); |
amandaghassaei | 10:6167cc7e7d60 | 30 | |
amandaghassaei | 6:748bae310e1e | 31 | // wait for MPU interrupt or extra packet(s) available |
amandaghassaei | 6:748bae310e1e | 32 | if (!mpuInterrupt && fifoCount < packetSize) { |
amandaghassaei | 6:748bae310e1e | 33 | return; |
amandaghassaei | 6:748bae310e1e | 34 | // other program behavior stuff here |
amandaghassaei | 6:748bae310e1e | 35 | // . |
amandaghassaei | 6:748bae310e1e | 36 | // . |
amandaghassaei | 6:748bae310e1e | 37 | // . |
amandaghassaei | 6:748bae310e1e | 38 | // if you are really paranoid you can frequently test in between other |
amandaghassaei | 6:748bae310e1e | 39 | // stuff to see if mpuInterrupt is true, and if so, "break;" from the |
amandaghassaei | 6:748bae310e1e | 40 | // while() loop to immediately process the MPU data |
amandaghassaei | 6:748bae310e1e | 41 | // . |
amandaghassaei | 6:748bae310e1e | 42 | // . |
amandaghassaei | 6:748bae310e1e | 43 | // . |
amandaghassaei | 6:748bae310e1e | 44 | } |
amandaghassaei | 10:6167cc7e7d60 | 45 | |
amandaghassaei | 10:6167cc7e7d60 | 46 | // _pc->printf("mpu interrupt \n"); |
amandaghassaei | 10:6167cc7e7d60 | 47 | |
amandaghassaei | 6:748bae310e1e | 48 | // reset interrupt flag and get INT_STATUS byte |
amandaghassaei | 6:748bae310e1e | 49 | mpuInterrupt = false; |
amandaghassaei | 6:748bae310e1e | 50 | mpuIntStatus = mpu.getIntStatus(); |
amandaghassaei | 6:748bae310e1e | 51 | |
amandaghassaei | 6:748bae310e1e | 52 | // get current FIFO count |
amandaghassaei | 6:748bae310e1e | 53 | fifoCount = mpu.getFIFOCount(); |
amandaghassaei | 6:748bae310e1e | 54 | |
amandaghassaei | 6:748bae310e1e | 55 | // check for overflow (this should never happen unless our code is too inefficient) |
amandaghassaei | 6:748bae310e1e | 56 | if ((mpuIntStatus & 0x10) || fifoCount == 1024) { |
amandaghassaei | 6:748bae310e1e | 57 | // reset so we can continue cleanly |
amandaghassaei | 6:748bae310e1e | 58 | mpu.resetFIFO(); |
amandaghassaei | 11:feab67f916fe | 59 | // _pc->printf("FIFO overflow!\n"); |
amandaghassaei | 6:748bae310e1e | 60 | |
amandaghassaei | 6:748bae310e1e | 61 | // otherwise, check for DMP data ready interrupt (this should happen frequently) |
amandaghassaei | 6:748bae310e1e | 62 | } else if (mpuIntStatus & 0x02) { |
amandaghassaei | 6:748bae310e1e | 63 | // wait for correct available data length, should be a VERY short wait |
amandaghassaei | 6:748bae310e1e | 64 | while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); |
amandaghassaei | 6:748bae310e1e | 65 | |
amandaghassaei | 6:748bae310e1e | 66 | // read a packet from FIFO |
amandaghassaei | 6:748bae310e1e | 67 | mpu.getFIFOBytes(fifoBuffer, packetSize); |
amandaghassaei | 6:748bae310e1e | 68 | |
amandaghassaei | 6:748bae310e1e | 69 | // track FIFO count here in case there is > 1 packet available |
amandaghassaei | 6:748bae310e1e | 70 | // (this lets us immediately read more without waiting for an interrupt) |
amandaghassaei | 6:748bae310e1e | 71 | fifoCount -= packetSize; |
amandaghassaei | 6:748bae310e1e | 72 | |
amandaghassaei | 6:748bae310e1e | 73 | // display Euler angles in degrees |
amandaghassaei | 6:748bae310e1e | 74 | mpu.dmpGetQuaternion(&q, fifoBuffer); |
amandaghassaei | 6:748bae310e1e | 75 | mpu.dmpGetGravity(&gravity, &q); |
amandaghassaei | 6:748bae310e1e | 76 | mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); |
amandaghassaei | 6:748bae310e1e | 77 | mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); |
amandaghassaei | 9:ac76b5b24b18 | 78 | float newTheta; |
amandaghassaei | 9:ac76b5b24b18 | 79 | if (ypr[1]>0) newTheta = ypr[2]; |
amandaghassaei | 9:ac76b5b24b18 | 80 | else if (ypr[2]<0) newTheta = -M_PI-ypr[2]; |
amandaghassaei | 9:ac76b5b24b18 | 81 | else newTheta = M_PI-ypr[2]; |
amandaghassaei | 9:ac76b5b24b18 | 82 | |
amandaghassaei | 9:ac76b5b24b18 | 83 | mpu.dmpGetGyro(gyroData, fifoBuffer); |
amandaghassaei | 11:feab67f916fe | 84 | float newDTheta = -gyroData[2]/180.0*M_PI; |
amandaghassaei | 9:ac76b5b24b18 | 85 | |
amandaghassaei | 9:ac76b5b24b18 | 86 | if (!thetaInitFlag && abs(newTheta-theta)>0.3) { |
amandaghassaei | 10:6167cc7e7d60 | 87 | // _pc->printf("IMU interrupt clash\n"); |
amandaghassaei | 9:ac76b5b24b18 | 88 | mpu.resetFIFO(); |
amandaghassaei | 9:ac76b5b24b18 | 89 | } else { |
amandaghassaei | 9:ac76b5b24b18 | 90 | thetaInitFlag = false; |
amandaghassaei | 9:ac76b5b24b18 | 91 | theta = newTheta; |
amandaghassaei | 9:ac76b5b24b18 | 92 | dtheta = newDTheta; |
amandaghassaei | 10:6167cc7e7d60 | 93 | // _pc->printf("loop %f\n", theta); |
amandaghassaei | 9:ac76b5b24b18 | 94 | } |
amandaghassaei | 8:96fa92188874 | 95 | |
amandaghassaei | 9:ac76b5b24b18 | 96 | |
amandaghassaei | 9:ac76b5b24b18 | 97 | |
amandaghassaei | 6:748bae310e1e | 98 | } |
amandaghassaei | 6:748bae310e1e | 99 | } |
amandaghassaei | 6:748bae310e1e | 100 | |
amandaghassaei | 6:748bae310e1e | 101 | void dmpDataReady() { |
amandaghassaei | 6:748bae310e1e | 102 | mpuInterrupt = true; |
amandaghassaei | 6:748bae310e1e | 103 | } |
amandaghassaei | 6:748bae310e1e | 104 | |
amandaghassaei | 6:748bae310e1e | 105 | float getTheta(){ |
amandaghassaei | 10:6167cc7e7d60 | 106 | // _pc->printf("%f\n", theta); |
amandaghassaei | 6:748bae310e1e | 107 | return theta; |
amandaghassaei | 6:748bae310e1e | 108 | } |
amandaghassaei | 6:748bae310e1e | 109 | |
amandaghassaei | 7:07f1f975429a | 110 | float getDTheta(){ |
amandaghassaei | 6:748bae310e1e | 111 | return dtheta; |
amandaghassaei | 6:748bae310e1e | 112 | } |
amandaghassaei | 8:96fa92188874 | 113 | |
amandaghassaei | 8:96fa92188874 | 114 | void disable(){ |
amandaghassaei | 8:96fa92188874 | 115 | dmpReady = false; |
amandaghassaei | 8:96fa92188874 | 116 | mpuInterrupt = false; |
amandaghassaei | 8:96fa92188874 | 117 | checkpin.rise(NULL); |
amandaghassaei | 8:96fa92188874 | 118 | } |
amandaghassaei | 8:96fa92188874 | 119 | |
amandaghassaei | 10:6167cc7e7d60 | 120 | void disableInterrupt(){ |
amandaghassaei | 10:6167cc7e7d60 | 121 | mpuInterrupt = false; |
amandaghassaei | 10:6167cc7e7d60 | 122 | checkpin.rise(NULL); |
amandaghassaei | 10:6167cc7e7d60 | 123 | } |
amandaghassaei | 10:6167cc7e7d60 | 124 | |
amandaghassaei | 8:96fa92188874 | 125 | void enable(){ |
amandaghassaei | 8:96fa92188874 | 126 | setup(); |
amandaghassaei | 8:96fa92188874 | 127 | } |
amandaghassaei | 10:6167cc7e7d60 | 128 | |
amandaghassaei | 10:6167cc7e7d60 | 129 | void enableInterrupt(){ |
amandaghassaei | 10:6167cc7e7d60 | 130 | checkpin.rise(this, &MyMPU6050::dmpDataReady); |
amandaghassaei | 10:6167cc7e7d60 | 131 | } |
amandaghassaei | 6:748bae310e1e | 132 | |
amandaghassaei | 6:748bae310e1e | 133 | private: |
amandaghassaei | 7:07f1f975429a | 134 | |
amandaghassaei | 8:96fa92188874 | 135 | Serial *_pc; |
amandaghassaei | 8:96fa92188874 | 136 | |
amandaghassaei | 6:748bae310e1e | 137 | MPU6050 mpu; |
amandaghassaei | 6:748bae310e1e | 138 | |
amandaghassaei | 6:748bae310e1e | 139 | bool dmpReady; // set true if DMP init was successful |
amandaghassaei | 6:748bae310e1e | 140 | uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU |
amandaghassaei | 6:748bae310e1e | 141 | uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) |
amandaghassaei | 6:748bae310e1e | 142 | uint16_t packetSize; // expected DMP packet size (default is 42 bytes) |
amandaghassaei | 6:748bae310e1e | 143 | uint16_t fifoCount; // count of all bytes currently in FIFO |
amandaghassaei | 6:748bae310e1e | 144 | uint8_t fifoBuffer[64]; // FIFO storage buffer |
amandaghassaei | 6:748bae310e1e | 145 | |
amandaghassaei | 6:748bae310e1e | 146 | // orientation/motion vars |
amandaghassaei | 6:748bae310e1e | 147 | Quaternion q; // [w, x, y, z] quaternion container |
amandaghassaei | 6:748bae310e1e | 148 | VectorFloat gravity; // [x, y, z] gravity vector |
amandaghassaei | 6:748bae310e1e | 149 | float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector |
amandaghassaei | 6:748bae310e1e | 150 | int16_t gyroData[3]; |
amandaghassaei | 10:6167cc7e7d60 | 151 | volatile float theta; |
amandaghassaei | 9:ac76b5b24b18 | 152 | bool thetaInitFlag; |
amandaghassaei | 10:6167cc7e7d60 | 153 | volatile float dtheta; |
amandaghassaei | 6:748bae310e1e | 154 | |
amandaghassaei | 6:748bae310e1e | 155 | InterruptIn checkpin; |
amandaghassaei | 6:748bae310e1e | 156 | volatile bool mpuInterrupt; |
amandaghassaei | 6:748bae310e1e | 157 | |
amandaghassaei | 7:07f1f975429a | 158 | |
amandaghassaei | 9:ac76b5b24b18 | 159 | void resetTheta(){ |
amandaghassaei | 9:ac76b5b24b18 | 160 | theta = 0; |
amandaghassaei | 9:ac76b5b24b18 | 161 | dtheta = 0; |
amandaghassaei | 9:ac76b5b24b18 | 162 | thetaInitFlag = true; |
amandaghassaei | 9:ac76b5b24b18 | 163 | } |
amandaghassaei | 9:ac76b5b24b18 | 164 | |
amandaghassaei | 6:748bae310e1e | 165 | void setup() { |
amandaghassaei | 6:748bae310e1e | 166 | |
amandaghassaei | 9:ac76b5b24b18 | 167 | resetTheta(); |
amandaghassaei | 6:748bae310e1e | 168 | |
amandaghassaei | 6:748bae310e1e | 169 | dmpReady = false; |
amandaghassaei | 6:748bae310e1e | 170 | mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high |
amandaghassaei | 6:748bae310e1e | 171 | |
amandaghassaei | 6:748bae310e1e | 172 | // initialize device |
amandaghassaei | 6:748bae310e1e | 173 | // pc.printf("Initializing I2C devices...\r\n"); |
amandaghassaei | 6:748bae310e1e | 174 | mpu.initialize(); |
amandaghassaei | 6:748bae310e1e | 175 | |
amandaghassaei | 6:748bae310e1e | 176 | // verify connection |
amandaghassaei | 6:748bae310e1e | 177 | // pc.printf("Testing device connections...\r\n"); |
amandaghassaei | 6:748bae310e1e | 178 | mpu.testConnection();//if() pc.printf("MPU6050 connection successful\r\n"); |
amandaghassaei | 6:748bae310e1e | 179 | // else pc.printf("MPU6050 connection failed\r\n"); |
amandaghassaei | 6:748bae310e1e | 180 | |
amandaghassaei | 6:748bae310e1e | 181 | // load and configure the DMP |
amandaghassaei | 6:748bae310e1e | 182 | // pc.printf("Initializing DMP...\r\n"); |
amandaghassaei | 6:748bae310e1e | 183 | devStatus = mpu.dmpInitialize(); |
amandaghassaei | 6:748bae310e1e | 184 | |
amandaghassaei | 6:748bae310e1e | 185 | // make sure it worked (returns 0 if so) |
amandaghassaei | 6:748bae310e1e | 186 | if (devStatus == 0) { |
amandaghassaei | 6:748bae310e1e | 187 | // turn on the DMP, now that it's ready |
amandaghassaei | 6:748bae310e1e | 188 | // pc.printf("Enabling DMP...\r\n"); |
amandaghassaei | 6:748bae310e1e | 189 | mpu.setDMPEnabled(true); |
amandaghassaei | 6:748bae310e1e | 190 | |
amandaghassaei | 6:748bae310e1e | 191 | // enable Arduino interrupt detection |
amandaghassaei | 6:748bae310e1e | 192 | // pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n"); |
amandaghassaei | 6:748bae310e1e | 193 | checkpin.rise(this, &MyMPU6050::dmpDataReady); |
amandaghassaei | 6:748bae310e1e | 194 | |
amandaghassaei | 6:748bae310e1e | 195 | mpuIntStatus = mpu.getIntStatus(); |
amandaghassaei | 6:748bae310e1e | 196 | |
amandaghassaei | 6:748bae310e1e | 197 | // set our DMP Ready flag so the main loop() function knows it's okay to use it |
amandaghassaei | 6:748bae310e1e | 198 | // pc.printf("DMP ready! Waiting for first interrupt...\r\n"); |
amandaghassaei | 6:748bae310e1e | 199 | dmpReady = true; |
amandaghassaei | 6:748bae310e1e | 200 | |
amandaghassaei | 6:748bae310e1e | 201 | // get expected DMP packet size for later comparison |
amandaghassaei | 6:748bae310e1e | 202 | packetSize = mpu.dmpGetFIFOPacketSize(); |
amandaghassaei | 6:748bae310e1e | 203 | } else { |
amandaghassaei | 6:748bae310e1e | 204 | // ERROR! |
amandaghassaei | 6:748bae310e1e | 205 | // 1 = initial memory load failed |
amandaghassaei | 6:748bae310e1e | 206 | // 2 = DMP configuration updates failed |
amandaghassaei | 6:748bae310e1e | 207 | // (if it's going to break, usually the code will be 1) |
amandaghassaei | 6:748bae310e1e | 208 | |
amandaghassaei | 6:748bae310e1e | 209 | // pc.printf("DDMP Initialization failed (code "); |
amandaghassaei | 6:748bae310e1e | 210 | // pc.printf("%d", devStatus); |
amandaghassaei | 6:748bae310e1e | 211 | // pc.printf(")\r\n"); |
amandaghassaei | 6:748bae310e1e | 212 | } |
amandaghassaei | 6:748bae310e1e | 213 | |
amandaghassaei | 6:748bae310e1e | 214 | } |
amandaghassaei | 6:748bae310e1e | 215 | }; |