What Is MPU6050?

Control a 3D model!

Oct 5, 2019

•

28502 views

•

2 respects

Components and supplies

Inertial Measurement Unit (IMU) (6 deg of freedom)

Arduino Nano R3

Apps and platforms

Arduino IDE

Processing

Project description

Code

untitled

c_cpp

1#include "MPU6050_6Axis_MotionApps20.h"
2//#include "MPU6050.h"   // not necessary if using MotionApps include file
3
4// Arduino Wire library   is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
5// is used in I2Cdev.h
6#if   I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
7    #include "Wire.h"
8#endif
9
10//   class default I2C address is 0x68
11// specific I2C addresses may be passed as   a parameter here
12// AD0 low = 0x68 (default for SparkFun breakout and InvenSense   evaluation board)
13// AD0 high = 0x69
14MPU6050 mpu;
15//MPU6050 mpu(0x69);   // <-- use for AD0 high
16
17/* =========================================================================
18    NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
19   depends   on the MPU-6050's INT pin being connected to the Arduino's
20   external interrupt   #0 pin. On the Arduino Uno and Mega 2560, this is
21   digital I/O pin 2.
22 *   ========================================================================= */
23
24/*   =========================================================================
25   NOTE:   Arduino v1.0.1 with the Leonardo board generates a compile error
26   when using   Serial.write(buf, len). The Teapot output uses this method.
27   The solution requires   a modification to the Arduino USBAPI.h file, which
28   is fortunately simple,   but annoying. This will be fixed in the next IDE
29   release. For more info, see   these links:
30
31   http://arduino.cc/forum/index.php/topic,109987.0.html
32    http://code.google.com/p/arduino/issues/detail?id=958
33 * =========================================================================   */
34
35
36
37// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see   the actual
38// quaternion components in a [w, x, y, z] format (not best for parsing
39//   on a remote host such as Processing or something though)
40//#define OUTPUT_READABLE_QUATERNION
41
42//   uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
43// (in degrees)   calculated from the quaternions coming from the FIFO.
44// Note that Euler angles   suffer from gimbal lock (for more info, see
45// http://en.wikipedia.org/wiki/Gimbal_lock)
46//#define   OUTPUT_READABLE_EULER
47
48// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you   want to see the yaw/
49// pitch/roll angles (in degrees) calculated from the quaternions   coming
50// from the FIFO. Note this also requires gravity vector calculations.
51//   Also note that yaw/pitch/roll angles suffer from gimbal lock (for
52// more info,   see: http://en.wikipedia.org/wiki/Gimbal_lock)
53#define OUTPUT_READABLE_YAWPITCHROLL
54
55//   uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
56// components   with gravity removed. This acceleration reference frame is
57// not compensated   for orientation, so +X is always +X according to the
58// sensor, just without   the effects of gravity. If you want acceleration
59// compensated for orientation,   us OUTPUT_READABLE_WORLDACCEL instead.
60//#define OUTPUT_READABLE_REALACCEL
61
62//   uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
63// components   with gravity removed and adjusted for the world frame of
64// reference (yaw is   relative to initial orientation, since no magnetometer
65// is present in this   case). Could be quite handy in some cases.
66//#define OUTPUT_READABLE_WORLDACCEL
67
68//   uncomment "OUTPUT_TEAPOT" if you want output that matches the
69// format used   for the InvenSense teapot demo
70//#define OUTPUT_TEAPOT
71
72
73
74#define   INTERRUPT_PIN 2  // use pin 2 on Arduino Uno & most boards
75#define LED_PIN 13   // (Arduino is 13, Teensy is 11, Teensy++ is 6)
76bool blinkState = false;
77
78//   MPU control/status vars
79bool dmpReady = false;  // set true if DMP init was successful
80uint8_t   mpuIntStatus;   // holds actual interrupt status byte from MPU
81uint8_t devStatus;       // return status after each device operation (0 = success, !0 = error)
82uint16_t   packetSize;    // expected DMP packet size (default is 42 bytes)
83uint16_t fifoCount;      // count of all bytes currently in FIFO
84uint8_t fifoBuffer[64]; // FIFO   storage buffer
85
86// orientation/motion vars
87Quaternion q;           // [w,   x, y, z]         quaternion container
88VectorInt16 aa;         // [x, y, z]            accel   sensor measurements
89VectorInt16 aaReal;     // [x, y, z]            gravity-free   accel sensor measurements
90VectorInt16 aaWorld;    // [x, y, z]            world-frame   accel sensor measurements
91VectorFloat gravity;    // [x, y, z]            gravity   vector
92float euler[3];         // [psi, theta, phi]    Euler angle container
93float   ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
94
95//   packet structure for InvenSense teapot demo
96uint8_t teapotPacket[14] = { '$',   0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\
', '\
97' };
98
99
100
101// ================================================================
102//   ===               INTERRUPT DETECTION ROUTINE                ===
103// ================================================================
104
105volatile   bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
106void   dmpDataReady() {
107    mpuInterrupt = true;
108}
109
110
111
112// ================================================================
113//   ===                      INITIAL SETUP                       ===
114// ================================================================
115
116void   setup() {
117    // join I2C bus (I2Cdev library doesn't do this automatically)
118     #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
119        Wire.begin();
120         Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having   compilation difficulties
121    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
122         Fastwire::setup(400, true);
123    #endif
124
125    // initialize serial   communication
126    // (115200 chosen because it is required for Teapot Demo output,   but it's
127    // really up to you depending on your project)
128    Serial.begin(115200);
129     while (!Serial); // wait for Leonardo enumeration, others continue immediately
130
131     // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3V or Arduino
132     // Pro Mini running at 3.3V, cannot handle this baud rate reliably due to
133     // the baud timing being too misaligned with processor ticks. You must use
134     // 38400 or slower in these cases, or use some kind of external separate
135     // crystal solution for the UART timer.
136
137    // initialize device
138     Serial.println(F("Initializing I2C devices..."));
139    mpu.initialize();
140     pinMode(INTERRUPT_PIN, INPUT);
141
142    // verify connection
143    Serial.println(F("Testing   device connections..."));
144    Serial.println(mpu.testConnection() ? F("MPU6050   connection successful") : F("MPU6050 connection failed"));
145
146    // wait   for ready
147    Serial.println(F("\
148Send any character to begin DMP programming   and demo: "));
149    while (Serial.available() && Serial.read()); // empty buffer
150     while (!Serial.available());                 // wait for data
151    while (Serial.available()   && Serial.read()); // empty buffer again
152
153    // load and configure the DMP
154     Serial.println(F("Initializing DMP..."));
155    devStatus = mpu.dmpInitialize();
156
157     // supply your own gyro offsets here, scaled for min sensitivity
158    mpu.setXGyroOffset(220);
159     mpu.setYGyroOffset(76);
160    mpu.setZGyroOffset(-85);
161    mpu.setZAccelOffset(1788);   // 1688 factory default for my test chip
162
163    // make sure it worked (returns   0 if so)
164    if (devStatus == 0) {
165        // Calibration Time: generate offsets   and calibrate our MPU6050
166        mpu.CalibrateAccel(6);
167        mpu.CalibrateGyro(6);
168         mpu.PrintActiveOffsets();
169        // turn on the DMP, now that it's ready
170         Serial.println(F("Enabling DMP..."));
171        mpu.setDMPEnabled(true);
172
173         // enable Arduino interrupt detection
174        Serial.print(F("Enabling   interrupt detection (Arduino external interrupt "));
175        Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
176         Serial.println(F(")..."));
177        attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN),   dmpDataReady, RISING);
178        mpuIntStatus = mpu.getIntStatus();
179
180        //   set our DMP Ready flag so the main loop() function knows it's okay to use it
181         Serial.println(F("DMP ready! Waiting for first interrupt..."));
182        dmpReady   = true;
183
184        // get expected DMP packet size for later comparison
185         packetSize = mpu.dmpGetFIFOPacketSize();
186    } else {
187        // ERROR!
188         // 1 = initial memory load failed
189        // 2 = DMP configuration updates   failed
190        // (if it's going to break, usually the code will be 1)
191        Serial.print(F("DMP   Initialization failed (code "));
192        Serial.print(devStatus);
193        Serial.println(F(")"));
194     }
195
196    // configure LED for output
197    pinMode(LED_PIN, OUTPUT);
198}
199
200
201
202//   ================================================================
203// ===                    MAIN   PROGRAM LOOP                     ===
204// ================================================================
205
206void   loop() {
207    // if programming failed, don't try to do anything
208    if (!dmpReady)   return;
209
210    // wait for MPU interrupt or extra packet(s) available
211    while   (!mpuInterrupt && fifoCount < packetSize) {
212        if (mpuInterrupt && fifoCount   < packetSize) {
213          // try to get out of the infinite loop 
214          fifoCount   = mpu.getFIFOCount();
215        }  
216        // other program behavior stuff   here
217        // .
218        // .
219        // .
220        // if you are really   paranoid you can frequently test in between other
221        // stuff to see if   mpuInterrupt is true, and if so, "break;" from the
222        // while() loop   to immediately process the MPU data
223        // .
224        // .
225        //   .
226    }
227
228    // reset interrupt flag and get INT_STATUS byte
229    mpuInterrupt   = false;
230    mpuIntStatus = mpu.getIntStatus();
231
232    // get current FIFO   count
233    fifoCount = mpu.getFIFOCount();
234	if(fifoCount < packetSize){
235	         //Lets go back and wait for another interrupt. We shouldn't be here, we   got an interrupt from another event
236			// This is blocking so don't do it    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
237	}
238    //   check for overflow (this should never happen unless our code is too inefficient)
239     else if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount   >= 1024) {
240        // reset so we can continue cleanly
241        mpu.resetFIFO();
242       //  fifoCount = mpu.getFIFOCount();  // will be zero after reset no need to   ask
243        Serial.println(F("FIFO overflow!"));
244
245    // otherwise, check   for DMP data ready interrupt (this should happen frequently)
246    } else if (mpuIntStatus   & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
247
248        // read a packet from FIFO
249	while(fifoCount   >= packetSize){ // Lets catch up to NOW, someone is using the dreaded delay()!
250		mpu.getFIFOBytes(fifoBuffer,   packetSize);
251		// track FIFO count here in case there is > 1 packet available
252		//   (this lets us immediately read more without waiting for an interrupt)
253		fifoCount   -= packetSize;
254	}
255        #ifdef OUTPUT_READABLE_QUATERNION
256            //   display quaternion values in easy matrix form: w x y z
257            mpu.dmpGetQuaternion(&q,   fifoBuffer);
258            Serial.print("quat\	");
259            Serial.print(q.w);
260             Serial.print("\	");
261            Serial.print(q.x);
262            Serial.print("\	");
263             Serial.print(q.y);
264            Serial.print("\	");
265            Serial.println(q.z);
266         #endif
267
268        #ifdef OUTPUT_READABLE_EULER
269            // display   Euler angles in degrees
270            mpu.dmpGetQuaternion(&q, fifoBuffer);
271             mpu.dmpGetEuler(euler, &q);
272            Serial.print("euler\	");
273             Serial.print(euler[0] * 180/M_PI);
274            Serial.print("\	");
275             Serial.print(euler[1] * 180/M_PI);
276            Serial.print("\	");
277             Serial.println(euler[2] * 180/M_PI);
278        #endif
279
280        #ifdef   OUTPUT_READABLE_YAWPITCHROLL
281            // display Euler angles in degrees
282             mpu.dmpGetQuaternion(&q, fifoBuffer);
283            mpu.dmpGetGravity(&gravity,   &q);
284            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
285            Serial.print("ypr\	");
286             Serial.print(ypr[0] * 180/M_PI);
287            Serial.print("\	");
288             Serial.print(ypr[1] * 180/M_PI);
289            Serial.print("\	");
290             Serial.println(ypr[2] * 180/M_PI);
291        #endif
292
293        #ifdef   OUTPUT_READABLE_REALACCEL
294            // display real acceleration, adjusted   to remove gravity
295            mpu.dmpGetQuaternion(&q, fifoBuffer);
296            mpu.dmpGetAccel(&aa,   fifoBuffer);
297            mpu.dmpGetGravity(&gravity, &q);
298            mpu.dmpGetLinearAccel(&aaReal,   &aa, &gravity);
299            Serial.print("areal\	");
300            Serial.print(aaReal.x);
301             Serial.print("\	");
302            Serial.print(aaReal.y);
303            Serial.print("\	");
304             Serial.println(aaReal.z);
305        #endif
306
307        #ifdef OUTPUT_READABLE_WORLDACCEL
308             // display initial world-frame acceleration, adjusted to remove gravity
309             // and rotated based on known orientation from quaternion
310            mpu.dmpGetQuaternion(&q,   fifoBuffer);
311            mpu.dmpGetAccel(&aa, fifoBuffer);
312            mpu.dmpGetGravity(&gravity,   &q);
313            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
314            mpu.dmpGetLinearAccelInWorld(&aaWorld,   &aaReal, &q);
315            Serial.print("aworld\	");
316            Serial.print(aaWorld.x);
317             Serial.print("\	");
318            Serial.print(aaWorld.y);
319            Serial.print("\	");
320             Serial.println(aaWorld.z);
321        #endif
322    
323        #ifdef   OUTPUT_TEAPOT
324            // display quaternion values in InvenSense Teapot demo   format:
325            teapotPacket[2] = fifoBuffer[0];
326            teapotPacket[3]   = fifoBuffer[1];
327            teapotPacket[4] = fifoBuffer[4];
328            teapotPacket[5]   = fifoBuffer[5];
329            teapotPacket[6] = fifoBuffer[8];
330            teapotPacket[7]   = fifoBuffer[9];
331            teapotPacket[8] = fifoBuffer[12];
332            teapotPacket[9]   = fifoBuffer[13];
333            Serial.write(teapotPacket, 14);
334            teapotPacket[11]++;   // packetCount, loops at 0xFF on purpose
335        #endif
336
337        // blink   LED to indicate activity
338        blinkState = !blinkState;
339        digitalWrite(LED_PIN,   blinkState);
340    }
341}

#include "MPU6050_6Axis_MotionApps20.h"
//#include "MPU6050.h"   // not necessary if using MotionApps include file

// Arduino Wire library   is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if   I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

//   class default I2C address is 0x68
// specific I2C addresses may be passed as   a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense   evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69);   // <-- use for AD0 high

/* =========================================================================
    NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
   depends   on the MPU-6050's INT pin being connected to the Arduino's
   external interrupt   #0 pin. On the Arduino Uno and Mega 2560, this is
   digital I/O pin 2.
 *   ========================================================================= */

/*   =========================================================================
   NOTE:   Arduino v1.0.1 with the Leonardo board generates a compile error
   when using   Serial.write(buf, len). The Teapot output uses this method.
   The solution requires   a modification to the Arduino USBAPI.h file, which
   is fortunately simple,   but annoying. This will be fixed in the next IDE
   release. For more info, see   these links:

   http://arduino.cc/forum/index.php/topic,109987.0.html
    http://code.google.com/p/arduino/issues/detail?id=958
 * =========================================================================   */



// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see   the actual
// quaternion components in a [w, x, y, z] format (not best for parsing
//   on a remote host such as Processing or something though)
//#define OUTPUT_READABLE_QUATERNION

//   uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
// (in degrees)   calculated from the quaternions coming from the FIFO.
// Note that Euler angles   suffer from gimbal lock (for more info, see
// http://en.wikipedia.org/wiki/Gimbal_lock)
//#define   OUTPUT_READABLE_EULER

// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you   want to see the yaw/
// pitch/roll angles (in degrees) calculated from the quaternions   coming
// from the FIFO. Note this also requires gravity vector calculations.
//   Also note that yaw/pitch/roll angles suffer from gimbal lock (for
// more info,   see: http://en.wikipedia.org/wiki/Gimbal_lock)
#define OUTPUT_READABLE_YAWPITCHROLL

//   uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
// components   with gravity removed. This acceleration reference frame is
// not compensated   for orientation, so +X is always +X according to the
// sensor, just without   the effects of gravity. If you want acceleration
// compensated for orientation,   us OUTPUT_READABLE_WORLDACCEL instead.
//#define OUTPUT_READABLE_REALACCEL

//   uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
// components   with gravity removed and adjusted for the world frame of
// reference (yaw is   relative to initial orientation, since no magnetometer
// is present in this   case). Could be quite handy in some cases.
//#define OUTPUT_READABLE_WORLDACCEL

//   uncomment "OUTPUT_TEAPOT" if you want output that matches the
// format used   for the InvenSense teapot demo
//#define OUTPUT_TEAPOT



#define   INTERRUPT_PIN 2  // use pin 2 on Arduino Uno & most boards
#define LED_PIN 13   // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;

//   MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t   mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;       // return status after each device operation (0 = success, !0 = error)
uint16_t   packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;      // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO   storage buffer

// orientation/motion vars
Quaternion q;           // [w,   x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel   sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free   accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame   accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity   vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float   ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

//   packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$',   0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\
', '\
' };



// ================================================================
//   ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

volatile   bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void   dmpDataReady() {
    mpuInterrupt = true;
}



// ================================================================
//   ===                      INITIAL SETUP                       ===
// ================================================================

void   setup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
     #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
         Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having   compilation difficulties
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
         Fastwire::setup(400, true);
    #endif

    // initialize serial   communication
    // (115200 chosen because it is required for Teapot Demo output,   but it's
    // really up to you depending on your project)
    Serial.begin(115200);
     while (!Serial); // wait for Leonardo enumeration, others continue immediately

     // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3V or Arduino
     // Pro Mini running at 3.3V, cannot handle this baud rate reliably due to
     // the baud timing being too misaligned with processor ticks. You must use
     // 38400 or slower in these cases, or use some kind of external separate
     // crystal solution for the UART timer.

    // initialize device
     Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();
     pinMode(INTERRUPT_PIN, INPUT);

    // verify connection
    Serial.println(F("Testing   device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050   connection successful") : F("MPU6050 connection failed"));

    // wait   for ready
    Serial.println(F("\
Send any character to begin DMP programming   and demo: "));
    while (Serial.available() && Serial.read()); // empty buffer
     while (!Serial.available());                 // wait for data
    while (Serial.available()   && Serial.read()); // empty buffer again

    // load and configure the DMP
     Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();

     // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(220);
     mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1788);   // 1688 factory default for my test chip

    // make sure it worked (returns   0 if so)
    if (devStatus == 0) {
        // Calibration Time: generate offsets   and calibrate our MPU6050
        mpu.CalibrateAccel(6);
        mpu.CalibrateGyro(6);
         mpu.PrintActiveOffsets();
        // turn on the DMP, now that it's ready
         Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

         // enable Arduino interrupt detection
        Serial.print(F("Enabling   interrupt detection (Arduino external interrupt "));
        Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
         Serial.println(F(")..."));
        attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN),   dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        //   set our DMP Ready flag so the main loop() function knows it's okay to use it
         Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady   = true;

        // get expected DMP packet size for later comparison
         packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
        // ERROR!
         // 1 = initial memory load failed
        // 2 = DMP configuration updates   failed
        // (if it's going to break, usually the code will be 1)
        Serial.print(F("DMP   Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
     }

    // configure LED for output
    pinMode(LED_PIN, OUTPUT);
}



//   ================================================================
// ===                    MAIN   PROGRAM LOOP                     ===
// ================================================================

void   loop() {
    // if programming failed, don't try to do anything
    if (!dmpReady)   return;

    // wait for MPU interrupt or extra packet(s) available
    while   (!mpuInterrupt && fifoCount < packetSize) {
        if (mpuInterrupt && fifoCount   < packetSize) {
          // try to get out of the infinite loop 
          fifoCount   = mpu.getFIFOCount();
        }  
        // other program behavior stuff   here
        // .
        // .
        // .
        // if you are really   paranoid you can frequently test in between other
        // stuff to see if   mpuInterrupt is true, and if so, "break;" from the
        // while() loop   to immediately process the MPU data
        // .
        // .
        //   .
    }

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt   = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO   count
    fifoCount = mpu.getFIFOCount();
	if(fifoCount < packetSize){
	         //Lets go back and wait for another interrupt. We shouldn't be here, we   got an interrupt from another event
			// This is blocking so don't do it    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
	}
    //   check for overflow (this should never happen unless our code is too inefficient)
     else if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount   >= 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();
       //  fifoCount = mpu.getFIFOCount();  // will be zero after reset no need to   ask
        Serial.println(F("FIFO overflow!"));

    // otherwise, check   for DMP data ready interrupt (this should happen frequently)
    } else if (mpuIntStatus   & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {

        // read a packet from FIFO
	while(fifoCount   >= packetSize){ // Lets catch up to NOW, someone is using the dreaded delay()!
		mpu.getFIFOBytes(fifoBuffer,   packetSize);
		// track FIFO count here in case there is > 1 packet available
		//   (this lets us immediately read more without waiting for an interrupt)
		fifoCount   -= packetSize;
	}
        #ifdef OUTPUT_READABLE_QUATERNION
            //   display quaternion values in easy matrix form: w x y z
            mpu.dmpGetQuaternion(&q,   fifoBuffer);
            Serial.print("quat\	");
            Serial.print(q.w);
             Serial.print("\	");
            Serial.print(q.x);
            Serial.print("\	");
             Serial.print(q.y);
            Serial.print("\	");
            Serial.println(q.z);
         #endif

        #ifdef OUTPUT_READABLE_EULER
            // display   Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
             mpu.dmpGetEuler(euler, &q);
            Serial.print("euler\	");
             Serial.print(euler[0] * 180/M_PI);
            Serial.print("\	");
             Serial.print(euler[1] * 180/M_PI);
            Serial.print("\	");
             Serial.println(euler[2] * 180/M_PI);
        #endif

        #ifdef   OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees
             mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity,   &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            Serial.print("ypr\	");
             Serial.print(ypr[0] * 180/M_PI);
            Serial.print("\	");
             Serial.print(ypr[1] * 180/M_PI);
            Serial.print("\	");
             Serial.println(ypr[2] * 180/M_PI);
        #endif

        #ifdef   OUTPUT_READABLE_REALACCEL
            // display real acceleration, adjusted   to remove gravity
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa,   fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal,   &aa, &gravity);
            Serial.print("areal\	");
            Serial.print(aaReal.x);
             Serial.print("\	");
            Serial.print(aaReal.y);
            Serial.print("\	");
             Serial.println(aaReal.z);
        #endif

        #ifdef OUTPUT_READABLE_WORLDACCEL
             // display initial world-frame acceleration, adjusted to remove gravity
             // and rotated based on known orientation from quaternion
            mpu.dmpGetQuaternion(&q,   fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity,   &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            mpu.dmpGetLinearAccelInWorld(&aaWorld,   &aaReal, &q);
            Serial.print("aworld\	");
            Serial.print(aaWorld.x);
             Serial.print("\	");
            Serial.print(aaWorld.y);
            Serial.print("\	");
             Serial.println(aaWorld.z);
        #endif
    
        #ifdef   OUTPUT_TEAPOT
            // display quaternion values in InvenSense Teapot demo   format:
            teapotPacket[2] = fifoBuffer[0];
            teapotPacket[3]   = fifoBuffer[1];
            teapotPacket[4] = fifoBuffer[4];
            teapotPacket[5]   = fifoBuffer[5];
            teapotPacket[6] = fifoBuffer[8];
            teapotPacket[7]   = fifoBuffer[9];
            teapotPacket[8] = fifoBuffer[12];
            teapotPacket[9]   = fifoBuffer[13];
            Serial.write(teapotPacket, 14);
            teapotPacket[11]++;   // packetCount, loops at 0xFF on purpose
        #endif

        // blink   LED to indicate activity
        blinkState = !blinkState;
        digitalWrite(LED_PIN,   blinkState);
    }
}

untitled

c_cpp

1#include "MPU6050_6Axis_MotionApps20.h"
2//#include "MPU6050.h"
3  // not necessary if using MotionApps include file
4
5// Arduino Wire library
6  is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
7// is used in I2Cdev.h
8#if
9  I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
10    #include "Wire.h"
11#endif
12
13//
14  class default I2C address is 0x68
15// specific I2C addresses may be passed as
16  a parameter here
17// AD0 low = 0x68 (default for SparkFun breakout and InvenSense
18  evaluation board)
19// AD0 high = 0x69
20MPU6050 mpu;
21//MPU6050 mpu(0x69);
22  // <-- use for AD0 high
23
24/* =========================================================================
25
26   NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
27   depends
28  on the MPU-6050's INT pin being connected to the Arduino's
29   external interrupt
30  #0 pin. On the Arduino Uno and Mega 2560, this is
31   digital I/O pin 2.
32 *
33  ========================================================================= */
34
35/*
36  =========================================================================
37   NOTE:
38  Arduino v1.0.1 with the Leonardo board generates a compile error
39   when using
40  Serial.write(buf, len). The Teapot output uses this method.
41   The solution requires
42  a modification to the Arduino USBAPI.h file, which
43   is fortunately simple,
44  but annoying. This will be fixed in the next IDE
45   release. For more info, see
46  these links:
47
48   http://arduino.cc/forum/index.php/topic,109987.0.html
49
50   http://code.google.com/p/arduino/issues/detail?id=958
51 * =========================================================================
52  */
53
54
55
56// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see
57  the actual
58// quaternion components in a [w, x, y, z] format (not best for parsing
59//
60  on a remote host such as Processing or something though)
61//#define OUTPUT_READABLE_QUATERNION
62
63//
64  uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
65// (in degrees)
66  calculated from the quaternions coming from the FIFO.
67// Note that Euler angles
68  suffer from gimbal lock (for more info, see
69// http://en.wikipedia.org/wiki/Gimbal_lock)
70//#define
71  OUTPUT_READABLE_EULER
72
73// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you
74  want to see the yaw/
75// pitch/roll angles (in degrees) calculated from the quaternions
76  coming
77// from the FIFO. Note this also requires gravity vector calculations.
78//
79  Also note that yaw/pitch/roll angles suffer from gimbal lock (for
80// more info,
81  see: http://en.wikipedia.org/wiki/Gimbal_lock)
82#define OUTPUT_READABLE_YAWPITCHROLL
83
84//
85  uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
86// components
87  with gravity removed. This acceleration reference frame is
88// not compensated
89  for orientation, so +X is always +X according to the
90// sensor, just without
91  the effects of gravity. If you want acceleration
92// compensated for orientation,
93  us OUTPUT_READABLE_WORLDACCEL instead.
94//#define OUTPUT_READABLE_REALACCEL
95
96//
97  uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
98// components
99  with gravity removed and adjusted for the world frame of
100// reference (yaw is
101  relative to initial orientation, since no magnetometer
102// is present in this
103  case). Could be quite handy in some cases.
104//#define OUTPUT_READABLE_WORLDACCEL
105
106//
107  uncomment "OUTPUT_TEAPOT" if you want output that matches the
108// format used
109  for the InvenSense teapot demo
110//#define OUTPUT_TEAPOT
111
112
113
114#define
115  INTERRUPT_PIN 2  // use pin 2 on Arduino Uno & most boards
116#define LED_PIN 13
117  // (Arduino is 13, Teensy is 11, Teensy++ is 6)
118bool blinkState = false;
119
120//
121  MPU control/status vars
122bool dmpReady = false;  // set true if DMP init was successful
123uint8_t
124  mpuIntStatus;   // holds actual interrupt status byte from MPU
125uint8_t devStatus;
126      // return status after each device operation (0 = success, !0 = error)
127uint16_t
128  packetSize;    // expected DMP packet size (default is 42 bytes)
129uint16_t fifoCount;
130     // count of all bytes currently in FIFO
131uint8_t fifoBuffer[64]; // FIFO
132  storage buffer
133
134// orientation/motion vars
135Quaternion q;           // [w,
136  x, y, z]         quaternion container
137VectorInt16 aa;         // [x, y, z]            accel
138  sensor measurements
139VectorInt16 aaReal;     // [x, y, z]            gravity-free
140  accel sensor measurements
141VectorInt16 aaWorld;    // [x, y, z]            world-frame
142  accel sensor measurements
143VectorFloat gravity;    // [x, y, z]            gravity
144  vector
145float euler[3];         // [psi, theta, phi]    Euler angle container
146float
147  ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
148
149//
150  packet structure for InvenSense teapot demo
151uint8_t teapotPacket[14] = { '$',
152  0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\
', '\
153' };
154
155
156
157// ================================================================
158//
159  ===               INTERRUPT DETECTION ROUTINE                ===
160// ================================================================
161
162volatile
163  bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
164void
165  dmpDataReady() {
166    mpuInterrupt = true;
167}
168
169
170
171// ================================================================
172//
173  ===                      INITIAL SETUP                       ===
174// ================================================================
175
176void
177  setup() {
178    // join I2C bus (I2Cdev library doesn't do this automatically)
179
180    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
181        Wire.begin();
182
183        Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having
184  compilation difficulties
185    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
186
187        Fastwire::setup(400, true);
188    #endif
189
190    // initialize serial
191  communication
192    // (115200 chosen because it is required for Teapot Demo output,
193  but it's
194    // really up to you depending on your project)
195    Serial.begin(115200);
196
197    while (!Serial); // wait for Leonardo enumeration, others continue immediately
198
199
200    // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3V or Arduino
201
202    // Pro Mini running at 3.3V, cannot handle this baud rate reliably due to
203
204    // the baud timing being too misaligned with processor ticks. You must use
205
206    // 38400 or slower in these cases, or use some kind of external separate
207
208    // crystal solution for the UART timer.
209
210    // initialize device
211
212    Serial.println(F("Initializing I2C devices..."));
213    mpu.initialize();
214
215    pinMode(INTERRUPT_PIN, INPUT);
216
217    // verify connection
218    Serial.println(F("Testing
219  device connections..."));
220    Serial.println(mpu.testConnection() ? F("MPU6050
221  connection successful") : F("MPU6050 connection failed"));
222
223    // wait
224  for ready
225    Serial.println(F("\
226Send any character to begin DMP programming
227  and demo: "));
228    while (Serial.available() && Serial.read()); // empty buffer
229
230    while (!Serial.available());                 // wait for data
231    while (Serial.available()
232  && Serial.read()); // empty buffer again
233
234    // load and configure the DMP
235
236    Serial.println(F("Initializing DMP..."));
237    devStatus = mpu.dmpInitialize();
238
239
240    // supply your own gyro offsets here, scaled for min sensitivity
241    mpu.setXGyroOffset(220);
242
243    mpu.setYGyroOffset(76);
244    mpu.setZGyroOffset(-85);
245    mpu.setZAccelOffset(1788);
246  // 1688 factory default for my test chip
247
248    // make sure it worked (returns
249  0 if so)
250    if (devStatus == 0) {
251        // Calibration Time: generate offsets
252  and calibrate our MPU6050
253        mpu.CalibrateAccel(6);
254        mpu.CalibrateGyro(6);
255
256        mpu.PrintActiveOffsets();
257        // turn on the DMP, now that it's ready
258
259        Serial.println(F("Enabling DMP..."));
260        mpu.setDMPEnabled(true);
261
262
263        // enable Arduino interrupt detection
264        Serial.print(F("Enabling
265  interrupt detection (Arduino external interrupt "));
266        Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
267
268        Serial.println(F(")..."));
269        attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN),
270  dmpDataReady, RISING);
271        mpuIntStatus = mpu.getIntStatus();
272
273        //
274  set our DMP Ready flag so the main loop() function knows it's okay to use it
275
276        Serial.println(F("DMP ready! Waiting for first interrupt..."));
277        dmpReady
278  = true;
279
280        // get expected DMP packet size for later comparison
281
282        packetSize = mpu.dmpGetFIFOPacketSize();
283    } else {
284        // ERROR!
285
286        // 1 = initial memory load failed
287        // 2 = DMP configuration updates
288  failed
289        // (if it's going to break, usually the code will be 1)
290        Serial.print(F("DMP
291  Initialization failed (code "));
292        Serial.print(devStatus);
293        Serial.println(F(")"));
294
295    }
296
297    // configure LED for output
298    pinMode(LED_PIN, OUTPUT);
299}
300
301
302
303//
304  ================================================================
305// ===                    MAIN
306  PROGRAM LOOP                     ===
307// ================================================================
308
309void
310  loop() {
311    // if programming failed, don't try to do anything
312    if (!dmpReady)
313  return;
314
315    // wait for MPU interrupt or extra packet(s) available
316    while
317  (!mpuInterrupt && fifoCount < packetSize) {
318        if (mpuInterrupt && fifoCount
319  < packetSize) {
320          // try to get out of the infinite loop 
321          fifoCount
322  = mpu.getFIFOCount();
323        }  
324        // other program behavior stuff
325  here
326        // .
327        // .
328        // .
329        // if you are really
330  paranoid you can frequently test in between other
331        // stuff to see if
332  mpuInterrupt is true, and if so, "break;" from the
333        // while() loop
334  to immediately process the MPU data
335        // .
336        // .
337        //
338  .
339    }
340
341    // reset interrupt flag and get INT_STATUS byte
342    mpuInterrupt
343  = false;
344    mpuIntStatus = mpu.getIntStatus();
345
346    // get current FIFO
347  count
348    fifoCount = mpu.getFIFOCount();
349	if(fifoCount < packetSize){
350	
351        //Lets go back and wait for another interrupt. We shouldn't be here, we
352  got an interrupt from another event
353			// This is blocking so don't do it
354   while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
355	}
356    //
357  check for overflow (this should never happen unless our code is too inefficient)
358
359    else if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount
360  >= 1024) {
361        // reset so we can continue cleanly
362        mpu.resetFIFO();
363
364      //  fifoCount = mpu.getFIFOCount();  // will be zero after reset no need to
365  ask
366        Serial.println(F("FIFO overflow!"));
367
368    // otherwise, check
369  for DMP data ready interrupt (this should happen frequently)
370    } else if (mpuIntStatus
371  & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
372
373        // read a packet from FIFO
374	while(fifoCount
375  >= packetSize){ // Lets catch up to NOW, someone is using the dreaded delay()!
376		mpu.getFIFOBytes(fifoBuffer,
377  packetSize);
378		// track FIFO count here in case there is > 1 packet available
379		//
380  (this lets us immediately read more without waiting for an interrupt)
381		fifoCount
382  -= packetSize;
383	}
384        #ifdef OUTPUT_READABLE_QUATERNION
385            //
386  display quaternion values in easy matrix form: w x y z
387            mpu.dmpGetQuaternion(&q,
388  fifoBuffer);
389            Serial.print("quat\	");
390            Serial.print(q.w);
391
392            Serial.print("\	");
393            Serial.print(q.x);
394            Serial.print("\	");
395
396            Serial.print(q.y);
397            Serial.print("\	");
398            Serial.println(q.z);
399
400        #endif
401
402        #ifdef OUTPUT_READABLE_EULER
403            // display
404  Euler angles in degrees
405            mpu.dmpGetQuaternion(&q, fifoBuffer);
406
407            mpu.dmpGetEuler(euler, &q);
408            Serial.print("euler\	");
409
410            Serial.print(euler[0] * 180/M_PI);
411            Serial.print("\	");
412
413            Serial.print(euler[1] * 180/M_PI);
414            Serial.print("\	");
415
416            Serial.println(euler[2] * 180/M_PI);
417        #endif
418
419        #ifdef
420  OUTPUT_READABLE_YAWPITCHROLL
421            // display Euler angles in degrees
422
423            mpu.dmpGetQuaternion(&q, fifoBuffer);
424            mpu.dmpGetGravity(&gravity,
425  &q);
426            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
427            Serial.print("ypr\	");
428
429            Serial.print(ypr[0] * 180/M_PI);
430            Serial.print("\	");
431
432            Serial.print(ypr[1] * 180/M_PI);
433            Serial.print("\	");
434
435            Serial.println(ypr[2] * 180/M_PI);
436        #endif
437
438        #ifdef
439  OUTPUT_READABLE_REALACCEL
440            // display real acceleration, adjusted
441  to remove gravity
442            mpu.dmpGetQuaternion(&q, fifoBuffer);
443            mpu.dmpGetAccel(&aa,
444  fifoBuffer);
445            mpu.dmpGetGravity(&gravity, &q);
446            mpu.dmpGetLinearAccel(&aaReal,
447  &aa, &gravity);
448            Serial.print("areal\	");
449            Serial.print(aaReal.x);
450
451            Serial.print("\	");
452            Serial.print(aaReal.y);
453            Serial.print("\	");
454
455            Serial.println(aaReal.z);
456        #endif
457
458        #ifdef OUTPUT_READABLE_WORLDACCEL
459
460            // display initial world-frame acceleration, adjusted to remove gravity
461
462            // and rotated based on known orientation from quaternion
463            mpu.dmpGetQuaternion(&q,
464  fifoBuffer);
465            mpu.dmpGetAccel(&aa, fifoBuffer);
466            mpu.dmpGetGravity(&gravity,
467  &q);
468            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
469            mpu.dmpGetLinearAccelInWorld(&aaWorld,
470  &aaReal, &q);
471            Serial.print("aworld\	");
472            Serial.print(aaWorld.x);
473
474            Serial.print("\	");
475            Serial.print(aaWorld.y);
476            Serial.print("\	");
477
478            Serial.println(aaWorld.z);
479        #endif
480    
481        #ifdef
482  OUTPUT_TEAPOT
483            // display quaternion values in InvenSense Teapot demo
484  format:
485            teapotPacket[2] = fifoBuffer[0];
486            teapotPacket[3]
487  = fifoBuffer[1];
488            teapotPacket[4] = fifoBuffer[4];
489            teapotPacket[5]
490  = fifoBuffer[5];
491            teapotPacket[6] = fifoBuffer[8];
492            teapotPacket[7]
493  = fifoBuffer[9];
494            teapotPacket[8] = fifoBuffer[12];
495            teapotPacket[9]
496  = fifoBuffer[13];
497            Serial.write(teapotPacket, 14);
498            teapotPacket[11]++;
499  // packetCount, loops at 0xFF on purpose
500        #endif
501
502        // blink
503  LED to indicate activity
504        blinkState = !blinkState;
505        digitalWrite(LED_PIN,
506  blinkState);
507    }
508}

#include "MPU6050_6Axis_MotionApps20.h"
//#include "MPU6050.h"
  // not necessary if using MotionApps include file

// Arduino Wire library
  is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if
  I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

//
  class default I2C address is 0x68
// specific I2C addresses may be passed as
  a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense
  evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69);
  // <-- use for AD0 high

/* =========================================================================

   NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
   depends
  on the MPU-6050's INT pin being connected to the Arduino's
   external interrupt
  #0 pin. On the Arduino Uno and Mega 2560, this is
   digital I/O pin 2.
 *
  ========================================================================= */

/*
  =========================================================================
   NOTE:
  Arduino v1.0.1 with the Leonardo board generates a compile error
   when using
  Serial.write(buf, len). The Teapot output uses this method.
   The solution requires
  a modification to the Arduino USBAPI.h file, which
   is fortunately simple,
  but annoying. This will be fixed in the next IDE
   release. For more info, see
  these links:

   http://arduino.cc/forum/index.php/topic,109987.0.html

   http://code.google.com/p/arduino/issues/detail?id=958
 * =========================================================================
  */



// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see
  the actual
// quaternion components in a [w, x, y, z] format (not best for parsing
//
  on a remote host such as Processing or something though)
//#define OUTPUT_READABLE_QUATERNION

//
  uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
// (in degrees)
  calculated from the quaternions coming from the FIFO.
// Note that Euler angles
  suffer from gimbal lock (for more info, see
// http://en.wikipedia.org/wiki/Gimbal_lock)
//#define
  OUTPUT_READABLE_EULER

// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you
  want to see the yaw/
// pitch/roll angles (in degrees) calculated from the quaternions
  coming
// from the FIFO. Note this also requires gravity vector calculations.
//
  Also note that yaw/pitch/roll angles suffer from gimbal lock (for
// more info,
  see: http://en.wikipedia.org/wiki/Gimbal_lock)
#define OUTPUT_READABLE_YAWPITCHROLL

//
  uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
// components
  with gravity removed. This acceleration reference frame is
// not compensated
  for orientation, so +X is always +X according to the
// sensor, just without
  the effects of gravity. If you want acceleration
// compensated for orientation,
  us OUTPUT_READABLE_WORLDACCEL instead.
//#define OUTPUT_READABLE_REALACCEL

//
  uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
// components
  with gravity removed and adjusted for the world frame of
// reference (yaw is
  relative to initial orientation, since no magnetometer
// is present in this
  case). Could be quite handy in some cases.
//#define OUTPUT_READABLE_WORLDACCEL

//
  uncomment "OUTPUT_TEAPOT" if you want output that matches the
// format used
  for the InvenSense teapot demo
//#define OUTPUT_TEAPOT



#define
  INTERRUPT_PIN 2  // use pin 2 on Arduino Uno & most boards
#define LED_PIN 13
  // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;

//
  MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t
  mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;
      // return status after each device operation (0 = success, !0 = error)
uint16_t
  packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;
     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO
  storage buffer

// orientation/motion vars
Quaternion q;           // [w,
  x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel
  sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free
  accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame
  accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity
  vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float
  ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

//
  packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$',
  0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\
', '\
' };



// ================================================================
//
  ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

volatile
  bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void
  dmpDataReady() {
    mpuInterrupt = true;
}



// ================================================================
//
  ===                      INITIAL SETUP                       ===
// ================================================================

void
  setup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)

    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();

        Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having
  compilation difficulties
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE

        Fastwire::setup(400, true);
    #endif

    // initialize serial
  communication
    // (115200 chosen because it is required for Teapot Demo output,
  but it's
    // really up to you depending on your project)
    Serial.begin(115200);

    while (!Serial); // wait for Leonardo enumeration, others continue immediately


    // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3V or Arduino

    // Pro Mini running at 3.3V, cannot handle this baud rate reliably due to

    // the baud timing being too misaligned with processor ticks. You must use

    // 38400 or slower in these cases, or use some kind of external separate

    // crystal solution for the UART timer.

    // initialize device

    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();

    pinMode(INTERRUPT_PIN, INPUT);

    // verify connection
    Serial.println(F("Testing
  device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050
  connection successful") : F("MPU6050 connection failed"));

    // wait
  for ready
    Serial.println(F("\
Send any character to begin DMP programming
  and demo: "));
    while (Serial.available() && Serial.read()); // empty buffer

    while (!Serial.available());                 // wait for data
    while (Serial.available()
  && Serial.read()); // empty buffer again

    // load and configure the DMP

    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();


    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(220);

    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1788);
  // 1688 factory default for my test chip

    // make sure it worked (returns
  0 if so)
    if (devStatus == 0) {
        // Calibration Time: generate offsets
  and calibrate our MPU6050
        mpu.CalibrateAccel(6);
        mpu.CalibrateGyro(6);

        mpu.PrintActiveOffsets();
        // turn on the DMP, now that it's ready

        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);


        // enable Arduino interrupt detection
        Serial.print(F("Enabling
  interrupt detection (Arduino external interrupt "));
        Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));

        Serial.println(F(")..."));
        attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN),
  dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        //
  set our DMP Ready flag so the main loop() function knows it's okay to use it

        Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady
  = true;

        // get expected DMP packet size for later comparison

        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
        // ERROR!

        // 1 = initial memory load failed
        // 2 = DMP configuration updates
  failed
        // (if it's going to break, usually the code will be 1)
        Serial.print(F("DMP
  Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));

    }

    // configure LED for output
    pinMode(LED_PIN, OUTPUT);
}



//
  ================================================================
// ===                    MAIN
  PROGRAM LOOP                     ===
// ================================================================

void
  loop() {
    // if programming failed, don't try to do anything
    if (!dmpReady)
  return;

    // wait for MPU interrupt or extra packet(s) available
    while
  (!mpuInterrupt && fifoCount < packetSize) {
        if (mpuInterrupt && fifoCount
  < packetSize) {
          // try to get out of the infinite loop 
          fifoCount
  = mpu.getFIFOCount();
        }  
        // other program behavior stuff
  here
        // .
        // .
        // .
        // if you are really
  paranoid you can frequently test in between other
        // stuff to see if
  mpuInterrupt is true, and if so, "break;" from the
        // while() loop
  to immediately process the MPU data
        // .
        // .
        //
  .
    }

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt
  = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO
  count
    fifoCount = mpu.getFIFOCount();
	if(fifoCount < packetSize){
	
        //Lets go back and wait for another interrupt. We shouldn't be here, we
  got an interrupt from another event
			// This is blocking so don't do it
   while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
	}
    //
  check for overflow (this should never happen unless our code is too inefficient)

    else if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount
  >= 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();

      //  fifoCount = mpu.getFIFOCount();  // will be zero after reset no need to
  ask
        Serial.println(F("FIFO overflow!"));

    // otherwise, check
  for DMP data ready interrupt (this should happen frequently)
    } else if (mpuIntStatus
  & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {

        // read a packet from FIFO
	while(fifoCount
  >= packetSize){ // Lets catch up to NOW, someone is using the dreaded delay()!
		mpu.getFIFOBytes(fifoBuffer,
  packetSize);
		// track FIFO count here in case there is > 1 packet available
		//
  (this lets us immediately read more without waiting for an interrupt)
		fifoCount
  -= packetSize;
	}
        #ifdef OUTPUT_READABLE_QUATERNION
            //
  display quaternion values in easy matrix form: w x y z
            mpu.dmpGetQuaternion(&q,
  fifoBuffer);
            Serial.print("quat\	");
            Serial.print(q.w);

            Serial.print("\	");
            Serial.print(q.x);
            Serial.print("\	");

            Serial.print(q.y);
            Serial.print("\	");
            Serial.println(q.z);

        #endif

        #ifdef OUTPUT_READABLE_EULER
            // display
  Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);

            mpu.dmpGetEuler(euler, &q);
            Serial.print("euler\	");

            Serial.print(euler[0] * 180/M_PI);
            Serial.print("\	");

            Serial.print(euler[1] * 180/M_PI);
            Serial.print("\	");

            Serial.println(euler[2] * 180/M_PI);
        #endif

        #ifdef
  OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees

            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity,
  &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            Serial.print("ypr\	");

            Serial.print(ypr[0] * 180/M_PI);
            Serial.print("\	");

            Serial.print(ypr[1] * 180/M_PI);
            Serial.print("\	");

            Serial.println(ypr[2] * 180/M_PI);
        #endif

        #ifdef
  OUTPUT_READABLE_REALACCEL
            // display real acceleration, adjusted
  to remove gravity
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa,
  fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal,
  &aa, &gravity);
            Serial.print("areal\	");
            Serial.print(aaReal.x);

            Serial.print("\	");
            Serial.print(aaReal.y);
            Serial.print("\	");

            Serial.println(aaReal.z);
        #endif

        #ifdef OUTPUT_READABLE_WORLDACCEL

            // display initial world-frame acceleration, adjusted to remove gravity

            // and rotated based on known orientation from quaternion
            mpu.dmpGetQuaternion(&q,
  fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity,
  &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            mpu.dmpGetLinearAccelInWorld(&aaWorld,
  &aaReal, &q);
            Serial.print("aworld\	");
            Serial.print(aaWorld.x);

            Serial.print("\	");
            Serial.print(aaWorld.y);
            Serial.print("\	");

            Serial.println(aaWorld.z);
        #endif
    
        #ifdef
  OUTPUT_TEAPOT
            // display quaternion values in InvenSense Teapot demo
  format:
            teapotPacket[2] = fifoBuffer[0];
            teapotPacket[3]
  = fifoBuffer[1];
            teapotPacket[4] = fifoBuffer[4];
            teapotPacket[5]
  = fifoBuffer[5];
            teapotPacket[6] = fifoBuffer[8];
            teapotPacket[7]
  = fifoBuffer[9];
            teapotPacket[8] = fifoBuffer[12];
            teapotPacket[9]
  = fifoBuffer[13];
            Serial.write(teapotPacket, 14);
            teapotPacket[11]++;
  // packetCount, loops at 0xFF on purpose
        #endif

        // blink
  LED to indicate activity
        blinkState = !blinkState;
        digitalWrite(LED_PIN,
  blinkState);
    }
}

untitled

c_cpp

1#include "MPU6050_6Axis_MotionApps20.h"
2//#include "MPU6050.h"  // not necessary if using MotionApps include file
3
4// Arduino Wire library  is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
5// is used in I2Cdev.h
6#if  I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
7    #include "Wire.h"
8#endif
9
10//  class default I2C address is 0x68
11// specific I2C addresses may be passed as  a parameter here
12// AD0 low = 0x68 (default for SparkFun breakout and InvenSense  evaluation board)
13// AD0 high = 0x69
14MPU6050 mpu;
15//MPU6050 mpu(0x69);  // <-- use for AD0 high
16
17/* =========================================================================
18   NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
19   depends  on the MPU-6050's INT pin being connected to the Arduino's
20   external interrupt  #0 pin. On the Arduino Uno and Mega 2560, this is
21   digital I/O pin 2.
22 *  ========================================================================= */
23
24/*  =========================================================================
25   NOTE:  Arduino v1.0.1 with the Leonardo board generates a compile error
26   when using  Serial.write(buf, len). The Teapot output uses this method.
27   The solution requires  a modification to the Arduino USBAPI.h file, which
28   is fortunately simple,  but annoying. This will be fixed in the next IDE
29   release. For more info, see  these links:
30
31   http://arduino.cc/forum/index.php/topic,109987.0.html
32   http://code.google.com/p/arduino/issues/detail?id=958
33 * =========================================================================  */
34
35
36
37// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see  the actual
38// quaternion components in a [w, x, y, z] format (not best for parsing
39//  on a remote host such as Processing or something though)
40//#define OUTPUT_READABLE_QUATERNION
41
42//  uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
43// (in degrees)  calculated from the quaternions coming from the FIFO.
44// Note that Euler angles  suffer from gimbal lock (for more info, see
45// http://en.wikipedia.org/wiki/Gimbal_lock)
46//#define  OUTPUT_READABLE_EULER
47
48// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you  want to see the yaw/
49// pitch/roll angles (in degrees) calculated from the quaternions  coming
50// from the FIFO. Note this also requires gravity vector calculations.
51//  Also note that yaw/pitch/roll angles suffer from gimbal lock (for
52// more info,  see: http://en.wikipedia.org/wiki/Gimbal_lock)
53#define OUTPUT_READABLE_YAWPITCHROLL
54
55//  uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
56// components  with gravity removed. This acceleration reference frame is
57// not compensated  for orientation, so +X is always +X according to the
58// sensor, just without  the effects of gravity. If you want acceleration
59// compensated for orientation,  us OUTPUT_READABLE_WORLDACCEL instead.
60//#define OUTPUT_READABLE_REALACCEL
61
62//  uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
63// components  with gravity removed and adjusted for the world frame of
64// reference (yaw is  relative to initial orientation, since no magnetometer
65// is present in this  case). Could be quite handy in some cases.
66//#define OUTPUT_READABLE_WORLDACCEL
67
68//  uncomment "OUTPUT_TEAPOT" if you want output that matches the
69// format used  for the InvenSense teapot demo
70//#define OUTPUT_TEAPOT
71
72
73
74#define  INTERRUPT_PIN 2  // use pin 2 on Arduino Uno & most boards
75#define LED_PIN 13  // (Arduino is 13, Teensy is 11, Teensy++ is 6)
76bool blinkState = false;
77
78//  MPU control/status vars
79bool dmpReady = false;  // set true if DMP init was successful
80uint8_t  mpuIntStatus;   // holds actual interrupt status byte from MPU
81uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
82uint16_t  packetSize;    // expected DMP packet size (default is 42 bytes)
83uint16_t fifoCount;     // count of all bytes currently in FIFO
84uint8_t fifoBuffer[64]; // FIFO  storage buffer
85
86// orientation/motion vars
87Quaternion q;           // [w,  x, y, z]         quaternion container
88VectorInt16 aa;         // [x, y, z]            accel  sensor measurements
89VectorInt16 aaReal;     // [x, y, z]            gravity-free  accel sensor measurements
90VectorInt16 aaWorld;    // [x, y, z]            world-frame  accel sensor measurements
91VectorFloat gravity;    // [x, y, z]            gravity  vector
92float euler[3];         // [psi, theta, phi]    Euler angle container
93float  ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
94
95//  packet structure for InvenSense teapot demo
96uint8_t teapotPacket[14] = { '$',  0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
97
98
99
100// ================================================================
101//  ===               INTERRUPT DETECTION ROUTINE                ===
102// ================================================================
103
104volatile  bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
105void  dmpDataReady() {
106    mpuInterrupt = true;
107}
108
109
110
111// ================================================================
112//  ===                      INITIAL SETUP                       ===
113// ================================================================
114
115void  setup() {
116    // join I2C bus (I2Cdev library doesn't do this automatically)
117    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
118        Wire.begin();
119        Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having  compilation difficulties
120    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
121        Fastwire::setup(400, true);
122    #endif
123
124    // initialize serial  communication
125    // (115200 chosen because it is required for Teapot Demo output,  but it's
126    // really up to you depending on your project)
127    Serial.begin(115200);
128    while (!Serial); // wait for Leonardo enumeration, others continue immediately
129
130    // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3V or Arduino
131    // Pro Mini running at 3.3V, cannot handle this baud rate reliably due to
132    // the baud timing being too misaligned with processor ticks. You must use
133    // 38400 or slower in these cases, or use some kind of external separate
134    // crystal solution for the UART timer.
135
136    // initialize device
137    Serial.println(F("Initializing I2C devices..."));
138    mpu.initialize();
139    pinMode(INTERRUPT_PIN, INPUT);
140
141    // verify connection
142    Serial.println(F("Testing  device connections..."));
143    Serial.println(mpu.testConnection() ? F("MPU6050  connection successful") : F("MPU6050 connection failed"));
144
145    // wait  for ready
146    Serial.println(F("\
147Send any character to begin DMP programming  and demo: "));
148    while (Serial.available() && Serial.read()); // empty buffer
149    while (!Serial.available());                 // wait for data
150    while (Serial.available()  && Serial.read()); // empty buffer again
151
152    // load and configure the DMP
153    Serial.println(F("Initializing DMP..."));
154    devStatus = mpu.dmpInitialize();
155
156    // supply your own gyro offsets here, scaled for min sensitivity
157    mpu.setXGyroOffset(220);
158    mpu.setYGyroOffset(76);
159    mpu.setZGyroOffset(-85);
160    mpu.setZAccelOffset(1788);  // 1688 factory default for my test chip
161
162    // make sure it worked (returns  0 if so)
163    if (devStatus == 0) {
164        // Calibration Time: generate offsets  and calibrate our MPU6050
165        mpu.CalibrateAccel(6);
166        mpu.CalibrateGyro(6);
167        mpu.PrintActiveOffsets();
168        // turn on the DMP, now that it's ready
169        Serial.println(F("Enabling DMP..."));
170        mpu.setDMPEnabled(true);
171
172        // enable Arduino interrupt detection
173        Serial.print(F("Enabling  interrupt detection (Arduino external interrupt "));
174        Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
175        Serial.println(F(")..."));
176        attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN),  dmpDataReady, RISING);
177        mpuIntStatus = mpu.getIntStatus();
178
179        //  set our DMP Ready flag so the main loop() function knows it's okay to use it
180        Serial.println(F("DMP ready! Waiting for first interrupt..."));
181        dmpReady  = true;
182
183        // get expected DMP packet size for later comparison
184        packetSize = mpu.dmpGetFIFOPacketSize();
185    } else {
186        // ERROR!
187        // 1 = initial memory load failed
188        // 2 = DMP configuration updates  failed
189        // (if it's going to break, usually the code will be 1)
190        Serial.print(F("DMP  Initialization failed (code "));
191        Serial.print(devStatus);
192        Serial.println(F(")"));
193    }
194
195    // configure LED for output
196    pinMode(LED_PIN, OUTPUT);
197}
198
199
200
201//  ================================================================
202// ===                    MAIN  PROGRAM LOOP                     ===
203// ================================================================
204
205void  loop() {
206    // if programming failed, don't try to do anything
207    if (!dmpReady)  return;
208
209    // wait for MPU interrupt or extra packet(s) available
210    while  (!mpuInterrupt && fifoCount < packetSize) {
211        if (mpuInterrupt && fifoCount  < packetSize) {
212          // try to get out of the infinite loop 
213          fifoCount  = mpu.getFIFOCount();
214        }  
215        // other program behavior stuff  here
216        // .
217        // .
218        // .
219        // if you are really  paranoid you can frequently test in between other
220        // stuff to see if  mpuInterrupt is true, and if so, "break;" from the
221        // while() loop  to immediately process the MPU data
222        // .
223        // .
224        //  .
225    }
226
227    // reset interrupt flag and get INT_STATUS byte
228    mpuInterrupt  = false;
229    mpuIntStatus = mpu.getIntStatus();
230
231    // get current FIFO  count
232    fifoCount = mpu.getFIFOCount();
233	if(fifoCount < packetSize){
234	        //Lets go back and wait for another interrupt. We shouldn't be here, we  got an interrupt from another event
235			// This is blocking so don't do it   while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
236	}
237    //  check for overflow (this should never happen unless our code is too inefficient)
238    else if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount  >= 1024) {
239        // reset so we can continue cleanly
240        mpu.resetFIFO();
241      //  fifoCount = mpu.getFIFOCount();  // will be zero after reset no need to  ask
242        Serial.println(F("FIFO overflow!"));
243
244    // otherwise, check  for DMP data ready interrupt (this should happen frequently)
245    } else if (mpuIntStatus  & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
246
247        // read a packet from FIFO
248	while(fifoCount  >= packetSize){ // Lets catch up to NOW, someone is using the dreaded delay()!
249		mpu.getFIFOBytes(fifoBuffer,  packetSize);
250		// track FIFO count here in case there is > 1 packet available
251		//  (this lets us immediately read more without waiting for an interrupt)
252		fifoCount  -= packetSize;
253	}
254        #ifdef OUTPUT_READABLE_QUATERNION
255            //  display quaternion values in easy matrix form: w x y z
256            mpu.dmpGetQuaternion(&q,  fifoBuffer);
257            Serial.print("quat\	");
258            Serial.print(q.w);
259            Serial.print("\	");
260            Serial.print(q.x);
261            Serial.print("\	");
262            Serial.print(q.y);
263            Serial.print("\	");
264            Serial.println(q.z);
265        #endif
266
267        #ifdef OUTPUT_READABLE_EULER
268            // display  Euler angles in degrees
269            mpu.dmpGetQuaternion(&q, fifoBuffer);
270            mpu.dmpGetEuler(euler, &q);
271            Serial.print("euler\	");
272            Serial.print(euler[0] * 180/M_PI);
273            Serial.print("\	");
274            Serial.print(euler[1] * 180/M_PI);
275            Serial.print("\	");
276            Serial.println(euler[2] * 180/M_PI);
277        #endif
278
279        #ifdef  OUTPUT_READABLE_YAWPITCHROLL
280            // display Euler angles in degrees
281            mpu.dmpGetQuaternion(&q, fifoBuffer);
282            mpu.dmpGetGravity(&gravity,  &q);
283            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
284            Serial.print("ypr\	");
285            Serial.print(ypr[0] * 180/M_PI);
286            Serial.print("\	");
287            Serial.print(ypr[1] * 180/M_PI);
288            Serial.print("\	");
289            Serial.println(ypr[2] * 180/M_PI);
290        #endif
291
292        #ifdef  OUTPUT_READABLE_REALACCEL
293            // display real acceleration, adjusted  to remove gravity
294            mpu.dmpGetQuaternion(&q, fifoBuffer);
295            mpu.dmpGetAccel(&aa,  fifoBuffer);
296            mpu.dmpGetGravity(&gravity, &q);
297            mpu.dmpGetLinearAccel(&aaReal,  &aa, &gravity);
298            Serial.print("areal\	");
299            Serial.print(aaReal.x);
300            Serial.print("\	");
301            Serial.print(aaReal.y);
302            Serial.print("\	");
303            Serial.println(aaReal.z);
304        #endif
305
306        #ifdef OUTPUT_READABLE_WORLDACCEL
307            // display initial world-frame acceleration, adjusted to remove gravity
308            // and rotated based on known orientation from quaternion
309            mpu.dmpGetQuaternion(&q,  fifoBuffer);
310            mpu.dmpGetAccel(&aa, fifoBuffer);
311            mpu.dmpGetGravity(&gravity,  &q);
312            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
313            mpu.dmpGetLinearAccelInWorld(&aaWorld,  &aaReal, &q);
314            Serial.print("aworld\	");
315            Serial.print(aaWorld.x);
316            Serial.print("\	");
317            Serial.print(aaWorld.y);
318            Serial.print("\	");
319            Serial.println(aaWorld.z);
320        #endif
321    
322        #ifdef  OUTPUT_TEAPOT
323            // display quaternion values in InvenSense Teapot demo  format:
324            teapotPacket[2] = fifoBuffer[0];
325            teapotPacket[3]  = fifoBuffer[1];
326            teapotPacket[4] = fifoBuffer[4];
327            teapotPacket[5]  = fifoBuffer[5];
328            teapotPacket[6] = fifoBuffer[8];
329            teapotPacket[7]  = fifoBuffer[9];
330            teapotPacket[8] = fifoBuffer[12];
331            teapotPacket[9]  = fifoBuffer[13];
332            Serial.write(teapotPacket, 14);
333            teapotPacket[11]++;  // packetCount, loops at 0xFF on purpose
334        #endif
335
336        // blink  LED to indicate activity
337        blinkState = !blinkState;
338        digitalWrite(LED_PIN,  blinkState);
339    }
340}

#include "MPU6050_6Axis_MotionApps20.h"
//#include "MPU6050.h"  // not necessary if using MotionApps include file

// Arduino Wire library  is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if  I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

//  class default I2C address is 0x68
// specific I2C addresses may be passed as  a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense  evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69);  // <-- use for AD0 high

/* =========================================================================
   NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
   depends  on the MPU-6050's INT pin being connected to the Arduino's
   external interrupt  #0 pin. On the Arduino Uno and Mega 2560, this is
   digital I/O pin 2.
 *  ========================================================================= */

/*  =========================================================================
   NOTE:  Arduino v1.0.1 with the Leonardo board generates a compile error
   when using  Serial.write(buf, len). The Teapot output uses this method.
   The solution requires  a modification to the Arduino USBAPI.h file, which
   is fortunately simple,  but annoying. This will be fixed in the next IDE
   release. For more info, see  these links:

   http://arduino.cc/forum/index.php/topic,109987.0.html
   http://code.google.com/p/arduino/issues/detail?id=958
 * =========================================================================  */



// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see  the actual
// quaternion components in a [w, x, y, z] format (not best for parsing
//  on a remote host such as Processing or something though)
//#define OUTPUT_READABLE_QUATERNION

//  uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
// (in degrees)  calculated from the quaternions coming from the FIFO.
// Note that Euler angles  suffer from gimbal lock (for more info, see
// http://en.wikipedia.org/wiki/Gimbal_lock)
//#define  OUTPUT_READABLE_EULER

// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you  want to see the yaw/
// pitch/roll angles (in degrees) calculated from the quaternions  coming
// from the FIFO. Note this also requires gravity vector calculations.
//  Also note that yaw/pitch/roll angles suffer from gimbal lock (for
// more info,  see: http://en.wikipedia.org/wiki/Gimbal_lock)
#define OUTPUT_READABLE_YAWPITCHROLL

//  uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
// components  with gravity removed. This acceleration reference frame is
// not compensated  for orientation, so +X is always +X according to the
// sensor, just without  the effects of gravity. If you want acceleration
// compensated for orientation,  us OUTPUT_READABLE_WORLDACCEL instead.
//#define OUTPUT_READABLE_REALACCEL

//  uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
// components  with gravity removed and adjusted for the world frame of
// reference (yaw is  relative to initial orientation, since no magnetometer
// is present in this  case). Could be quite handy in some cases.
//#define OUTPUT_READABLE_WORLDACCEL

//  uncomment "OUTPUT_TEAPOT" if you want output that matches the
// format used  for the InvenSense teapot demo
//#define OUTPUT_TEAPOT



#define  INTERRUPT_PIN 2  // use pin 2 on Arduino Uno & most boards
#define LED_PIN 13  // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;

//  MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t  mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t  packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO  storage buffer

// orientation/motion vars
Quaternion q;           // [w,  x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel  sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free  accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame  accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity  vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float  ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

//  packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$',  0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };



// ================================================================
//  ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

volatile  bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void  dmpDataReady() {
    mpuInterrupt = true;
}



// ================================================================
//  ===                      INITIAL SETUP                       ===
// ================================================================

void  setup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having  compilation difficulties
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    // initialize serial  communication
    // (115200 chosen because it is required for Teapot Demo output,  but it's
    // really up to you depending on your project)
    Serial.begin(115200);
    while (!Serial); // wait for Leonardo enumeration, others continue immediately

    // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3V or Arduino
    // Pro Mini running at 3.3V, cannot handle this baud rate reliably due to
    // the baud timing being too misaligned with processor ticks. You must use
    // 38400 or slower in these cases, or use some kind of external separate
    // crystal solution for the UART timer.

    // initialize device
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();
    pinMode(INTERRUPT_PIN, INPUT);

    // verify connection
    Serial.println(F("Testing  device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050  connection successful") : F("MPU6050 connection failed"));

    // wait  for ready
    Serial.println(F("\
Send any character to begin DMP programming  and demo: "));
    while (Serial.available() && Serial.read()); // empty buffer
    while (!Serial.available());                 // wait for data
    while (Serial.available()  && Serial.read()); // empty buffer again

    // load and configure the DMP
    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1788);  // 1688 factory default for my test chip

    // make sure it worked (returns  0 if so)
    if (devStatus == 0) {
        // Calibration Time: generate offsets  and calibrate our MPU6050
        mpu.CalibrateAccel(6);
        mpu.CalibrateGyro(6);
        mpu.PrintActiveOffsets();
        // turn on the DMP, now that it's ready
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        Serial.print(F("Enabling  interrupt detection (Arduino external interrupt "));
        Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
        Serial.println(F(")..."));
        attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN),  dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        //  set our DMP Ready flag so the main loop() function knows it's okay to use it
        Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady  = true;

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates  failed
        // (if it's going to break, usually the code will be 1)
        Serial.print(F("DMP  Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }

    // configure LED for output
    pinMode(LED_PIN, OUTPUT);
}



//  ================================================================
// ===                    MAIN  PROGRAM LOOP                     ===
// ================================================================

void  loop() {
    // if programming failed, don't try to do anything
    if (!dmpReady)  return;

    // wait for MPU interrupt or extra packet(s) available
    while  (!mpuInterrupt && fifoCount < packetSize) {
        if (mpuInterrupt && fifoCount  < packetSize) {
          // try to get out of the infinite loop 
          fifoCount  = mpu.getFIFOCount();
        }  
        // other program behavior stuff  here
        // .
        // .
        // .
        // if you are really  paranoid you can frequently test in between other
        // stuff to see if  mpuInterrupt is true, and if so, "break;" from the
        // while() loop  to immediately process the MPU data
        // .
        // .
        //  .
    }

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt  = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO  count
    fifoCount = mpu.getFIFOCount();
	if(fifoCount < packetSize){
	        //Lets go back and wait for another interrupt. We shouldn't be here, we  got an interrupt from another event
			// This is blocking so don't do it   while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
	}
    //  check for overflow (this should never happen unless our code is too inefficient)
    else if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount  >= 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();
      //  fifoCount = mpu.getFIFOCount();  // will be zero after reset no need to  ask
        Serial.println(F("FIFO overflow!"));

    // otherwise, check  for DMP data ready interrupt (this should happen frequently)
    } else if (mpuIntStatus  & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {

        // read a packet from FIFO
	while(fifoCount  >= packetSize){ // Lets catch up to NOW, someone is using the dreaded delay()!
		mpu.getFIFOBytes(fifoBuffer,  packetSize);
		// track FIFO count here in case there is > 1 packet available
		//  (this lets us immediately read more without waiting for an interrupt)
		fifoCount  -= packetSize;
	}
        #ifdef OUTPUT_READABLE_QUATERNION
            //  display quaternion values in easy matrix form: w x y z
            mpu.dmpGetQuaternion(&q,  fifoBuffer);
            Serial.print("quat\	");
            Serial.print(q.w);
            Serial.print("\	");
            Serial.print(q.x);
            Serial.print("\	");
            Serial.print(q.y);
            Serial.print("\	");
            Serial.println(q.z);
        #endif

        #ifdef OUTPUT_READABLE_EULER
            // display  Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetEuler(euler, &q);
            Serial.print("euler\	");
            Serial.print(euler[0] * 180/M_PI);
            Serial.print("\	");
            Serial.print(euler[1] * 180/M_PI);
            Serial.print("\	");
            Serial.println(euler[2] * 180/M_PI);
        #endif

        #ifdef  OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity,  &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            Serial.print("ypr\	");
            Serial.print(ypr[0] * 180/M_PI);
            Serial.print("\	");
            Serial.print(ypr[1] * 180/M_PI);
            Serial.print("\	");
            Serial.println(ypr[2] * 180/M_PI);
        #endif

        #ifdef  OUTPUT_READABLE_REALACCEL
            // display real acceleration, adjusted  to remove gravity
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa,  fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal,  &aa, &gravity);
            Serial.print("areal\	");
            Serial.print(aaReal.x);
            Serial.print("\	");
            Serial.print(aaReal.y);
            Serial.print("\	");
            Serial.println(aaReal.z);
        #endif

        #ifdef OUTPUT_READABLE_WORLDACCEL
            // display initial world-frame acceleration, adjusted to remove gravity
            // and rotated based on known orientation from quaternion
            mpu.dmpGetQuaternion(&q,  fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity,  &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            mpu.dmpGetLinearAccelInWorld(&aaWorld,  &aaReal, &q);
            Serial.print("aworld\	");
            Serial.print(aaWorld.x);
            Serial.print("\	");
            Serial.print(aaWorld.y);
            Serial.print("\	");
            Serial.println(aaWorld.z);
        #endif
    
        #ifdef  OUTPUT_TEAPOT
            // display quaternion values in InvenSense Teapot demo  format:
            teapotPacket[2] = fifoBuffer[0];
            teapotPacket[3]  = fifoBuffer[1];
            teapotPacket[4] = fifoBuffer[4];
            teapotPacket[5]  = fifoBuffer[5];
            teapotPacket[6] = fifoBuffer[8];
            teapotPacket[7]  = fifoBuffer[9];
            teapotPacket[8] = fifoBuffer[12];
            teapotPacket[9]  = fifoBuffer[13];
            Serial.write(teapotPacket, 14);
            teapotPacket[11]++;  // packetCount, loops at 0xFF on purpose
        #endif

        // blink  LED to indicate activity
        blinkState = !blinkState;
        digitalWrite(LED_PIN,  blinkState);
    }
}

Downloadable files

untitled

Comments

Only logged in users can leave comments

CiferTech

0 Followers

•

0 Projects

Intro

Trademarks & CopyrightsWhistleblowingDigital Services ActTerms of ServicePrivacy PolicySecurityCookie Settings

Report content