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