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