Shundo Kishi / Mbed 2 deprecated MPU6050_Example

Example to use MPU6050 library. This project contains usage of DMP and way to get raw values from the sensor. I ported here from this arduino's project https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050/Examples. See here if you want try visualization of sensor values. To get move this program, we need to connect pinName 27 to "SCL" pin, pinName 28 to "SDA" pin, and pinName 29 to "INT" pin of MPU6050.

Dependencies:   mbed MPU6050

main.cpp@0:8d2c753a96e7, 2013-11-19 (annotated)

Committer:
syundo0730
Date:
Tue Nov 19 19:10:30 2013 +0000
Revision:
0:8d2c753a96e7
Child:
1:ec0a08108442
first commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
syundo0730 0:8d2c753a96e7 1 // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
syundo0730 0:8d2c753a96e7 2 // 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
syundo0730 0:8d2c753a96e7 3 // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
syundo0730 0:8d2c753a96e7 4 //
syundo0730 0:8d2c753a96e7 5 // Changelog:
syundo0730 0:8d2c753a96e7 6 // 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
syundo0730 0:8d2c753a96e7 7 // 2012-06-20 - improved FIFO overflow handling and simplified read process
syundo0730 0:8d2c753a96e7 8 // 2012-06-19 - completely rearranged DMP initialization code and simplification
syundo0730 0:8d2c753a96e7 9 // 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
syundo0730 0:8d2c753a96e7 10 // 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
syundo0730 0:8d2c753a96e7 11 // 2012-06-05 - add gravity-compensated initial reference frame acceleration output
syundo0730 0:8d2c753a96e7 12 // - add 3D math helper file to DMP6 example sketch
syundo0730 0:8d2c753a96e7 13 // - add Euler output and Yaw/Pitch/Roll output formats
syundo0730 0:8d2c753a96e7 14 // 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
syundo0730 0:8d2c753a96e7 15 // 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
syundo0730 0:8d2c753a96e7 16 // 2012-05-30 - basic DMP initialization working
syundo0730 0:8d2c753a96e7 17
syundo0730 0:8d2c753a96e7 18 /* ============================================
syundo0730 0:8d2c753a96e7 19 I2Cdev device library code is placed under the MIT license
syundo0730 0:8d2c753a96e7 20 Copyright (c) 2012 Jeff Rowberg
syundo0730 0:8d2c753a96e7 21
syundo0730 0:8d2c753a96e7 22 Permission is hereby granted, free of charge, to any person obtaining a copy
syundo0730 0:8d2c753a96e7 23 of this software and associated documentation files (the "Software"), to deal
syundo0730 0:8d2c753a96e7 24 in the Software without restriction, including without limitation the rights
syundo0730 0:8d2c753a96e7 25 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
syundo0730 0:8d2c753a96e7 26 copies of the Software, and to permit persons to whom the Software is
syundo0730 0:8d2c753a96e7 27 furnished to do so, subject to the following conditions:
syundo0730 0:8d2c753a96e7 28
syundo0730 0:8d2c753a96e7 29 The above copyright notice and this permission notice shall be included in
syundo0730 0:8d2c753a96e7 30 all copies or substantial portions of the Software.
syundo0730 0:8d2c753a96e7 31
syundo0730 0:8d2c753a96e7 32 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
syundo0730 0:8d2c753a96e7 33 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
syundo0730 0:8d2c753a96e7 34 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
syundo0730 0:8d2c753a96e7 35 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
syundo0730 0:8d2c753a96e7 36 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
syundo0730 0:8d2c753a96e7 37 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
syundo0730 0:8d2c753a96e7 38 THE SOFTWARE.
syundo0730 0:8d2c753a96e7 39 ===============================================
syundo0730 0:8d2c753a96e7 40 */
syundo0730 0:8d2c753a96e7 41
syundo0730 0:8d2c753a96e7 42 // Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
syundo0730 0:8d2c753a96e7 43 // is used in I2Cdev.h
syundo0730 0:8d2c753a96e7 44 //#include "Wire.h"
syundo0730 0:8d2c753a96e7 45
syundo0730 0:8d2c753a96e7 46 // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
syundo0730 0:8d2c753a96e7 47 // for both classes must be in the include path of your project
syundo0730 0:8d2c753a96e7 48 #include "I2Cdev.h"
syundo0730 0:8d2c753a96e7 49
syundo0730 0:8d2c753a96e7 50 #include "MPU6050_6Axis_MotionApps20.h"
syundo0730 0:8d2c753a96e7 51 //#include "MPU6050.h" // not necessary if using MotionApps include file
syundo0730 0:8d2c753a96e7 52
syundo0730 0:8d2c753a96e7 53 // class default I2C address is 0x68
syundo0730 0:8d2c753a96e7 54 // specific I2C addresses may be passed as a parameter here
syundo0730 0:8d2c753a96e7 55 // AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
syundo0730 0:8d2c753a96e7 56 // AD0 high = 0x69
syundo0730 0:8d2c753a96e7 57 MPU6050 mpu;
syundo0730 0:8d2c753a96e7 58
syundo0730 0:8d2c753a96e7 59 /* =========================================================================
syundo0730 0:8d2c753a96e7 60 NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
syundo0730 0:8d2c753a96e7 61 depends on the MPU-6050's INT pin being connected to the Arduino's
syundo0730 0:8d2c753a96e7 62 external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
syundo0730 0:8d2c753a96e7 63 digital I/O pin 2.
syundo0730 0:8d2c753a96e7 64 * ========================================================================= */
syundo0730 0:8d2c753a96e7 65
syundo0730 0:8d2c753a96e7 66 /* =========================================================================
syundo0730 0:8d2c753a96e7 67 NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
syundo0730 0:8d2c753a96e7 68 when using Serial.write(buf, len). The Teapot output uses this method.
syundo0730 0:8d2c753a96e7 69 The solution requires a modification to the Arduino USBAPI.h file, which
syundo0730 0:8d2c753a96e7 70 is fortunately simple, but annoying. This will be fixed in the next IDE
syundo0730 0:8d2c753a96e7 71 release. For more info, see these links:
syundo0730 0:8d2c753a96e7 72
syundo0730 0:8d2c753a96e7 73 http://arduino.cc/forum/index.php/topic,109987.0.html
syundo0730 0:8d2c753a96e7 74 http://code.google.com/p/arduino/issues/detail?id=958
syundo0730 0:8d2c753a96e7 75 * ========================================================================= */
syundo0730 0:8d2c753a96e7 76
syundo0730 0:8d2c753a96e7 77
syundo0730 0:8d2c753a96e7 78 const float M_PI = 3.14159265;
syundo0730 0:8d2c753a96e7 79
syundo0730 0:8d2c753a96e7 80 // uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
syundo0730 0:8d2c753a96e7 81 // quaternion components in a [w, x, y, z] format (not best for parsing
syundo0730 0:8d2c753a96e7 82 // on a remote host such as Processing or something though)
syundo0730 0:8d2c753a96e7 83 //#define OUTPUT_READABLE_QUATERNION
syundo0730 0:8d2c753a96e7 84
syundo0730 0:8d2c753a96e7 85 // uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
syundo0730 0:8d2c753a96e7 86 // (in degrees) calculated from the quaternions coming from the FIFO.
syundo0730 0:8d2c753a96e7 87 // Note that Euler angles suffer from gimbal lock (for more info, see
syundo0730 0:8d2c753a96e7 88 // http://en.wikipedia.org/wiki/Gimbal_lock)
syundo0730 0:8d2c753a96e7 89 #define OUTPUT_READABLE_EULER
syundo0730 0:8d2c753a96e7 90
syundo0730 0:8d2c753a96e7 91 // uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
syundo0730 0:8d2c753a96e7 92 // pitch/roll angles (in degrees) calculated from the quaternions coming
syundo0730 0:8d2c753a96e7 93 // from the FIFO. Note this also requires gravity vector calculations.
syundo0730 0:8d2c753a96e7 94 // Also note that yaw/pitch/roll angles suffer from gimbal lock (for
syundo0730 0:8d2c753a96e7 95 // more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
syundo0730 0:8d2c753a96e7 96 //#define OUTPUT_READABLE_YAWPITCHROLL
syundo0730 0:8d2c753a96e7 97
syundo0730 0:8d2c753a96e7 98 // uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
syundo0730 0:8d2c753a96e7 99 // components with gravity removed. This acceleration reference frame is
syundo0730 0:8d2c753a96e7 100 // not compensated for orientation, so +X is always +X according to the
syundo0730 0:8d2c753a96e7 101 // sensor, just without the effects of gravity. If you want acceleration
syundo0730 0:8d2c753a96e7 102 // compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
syundo0730 0:8d2c753a96e7 103 //#define OUTPUT_READABLE_REALACCEL
syundo0730 0:8d2c753a96e7 104
syundo0730 0:8d2c753a96e7 105 // uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
syundo0730 0:8d2c753a96e7 106 // components with gravity removed and adjusted for the world frame of
syundo0730 0:8d2c753a96e7 107 // reference (yaw is relative to initial orientation, since no magnetometer
syundo0730 0:8d2c753a96e7 108 // is present in this case). Could be quite handy in some cases.
syundo0730 0:8d2c753a96e7 109 //#define OUTPUT_READABLE_WORLDACCEL
syundo0730 0:8d2c753a96e7 110
syundo0730 0:8d2c753a96e7 111 // uncomment "OUTPUT_TEAPOT" if you want output that matches the
syundo0730 0:8d2c753a96e7 112 // format used for the InvenSense teapot demo
syundo0730 0:8d2c753a96e7 113 //#define OUTPUT_TEAPOT
syundo0730 0:8d2c753a96e7 114
syundo0730 0:8d2c753a96e7 115
syundo0730 0:8d2c753a96e7 116
syundo0730 0:8d2c753a96e7 117 //#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
syundo0730 0:8d2c753a96e7 118 //bool blinkState = false;
syundo0730 0:8d2c753a96e7 119
syundo0730 0:8d2c753a96e7 120 // MPU control/status vars
syundo0730 0:8d2c753a96e7 121 bool dmpReady = false; // set true if DMP init was successful
syundo0730 0:8d2c753a96e7 122 uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
syundo0730 0:8d2c753a96e7 123 uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
syundo0730 0:8d2c753a96e7 124 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
syundo0730 0:8d2c753a96e7 125 uint16_t fifoCount; // count of all bytes currently in FIFO
syundo0730 0:8d2c753a96e7 126 uint8_t fifoBuffer[64]; // FIFO storage buffer
syundo0730 0:8d2c753a96e7 127
syundo0730 0:8d2c753a96e7 128 // orientation/motion vars
syundo0730 0:8d2c753a96e7 129 Quaternion q; // [w, x, y, z] quaternion container
syundo0730 0:8d2c753a96e7 130 VectorInt16 aa; // [x, y, z] accel sensor measurements
syundo0730 0:8d2c753a96e7 131 VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
syundo0730 0:8d2c753a96e7 132 VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
syundo0730 0:8d2c753a96e7 133 VectorFloat gravity; // [x, y, z] gravity vector
syundo0730 0:8d2c753a96e7 134 float euler[3]; // [psi, theta, phi] Euler angle container
syundo0730 0:8d2c753a96e7 135 float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
syundo0730 0:8d2c753a96e7 136
syundo0730 0:8d2c753a96e7 137 // packet structure for InvenSense teapot demo
syundo0730 0:8d2c753a96e7 138 uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
syundo0730 0:8d2c753a96e7 139
syundo0730 0:8d2c753a96e7 140 DigitalOut led1(LED1);
syundo0730 0:8d2c753a96e7 141
syundo0730 0:8d2c753a96e7 142 InterruptIn checkpin(p29);
syundo0730 0:8d2c753a96e7 143
syundo0730 0:8d2c753a96e7 144 Serial pc(USBTX, USBRX);
syundo0730 0:8d2c753a96e7 145
syundo0730 0:8d2c753a96e7 146 // ================================================================
syundo0730 0:8d2c753a96e7 147 // === INTERRUPT DETECTION ROUTINE ===
syundo0730 0:8d2c753a96e7 148 // ================================================================
syundo0730 0:8d2c753a96e7 149
syundo0730 0:8d2c753a96e7 150 volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
syundo0730 0:8d2c753a96e7 151 void dmpDataReady() {
syundo0730 0:8d2c753a96e7 152 mpuInterrupt = true;
syundo0730 0:8d2c753a96e7 153 }
syundo0730 0:8d2c753a96e7 154
syundo0730 0:8d2c753a96e7 155 void setup();
syundo0730 0:8d2c753a96e7 156 void loop();
syundo0730 0:8d2c753a96e7 157
syundo0730 0:8d2c753a96e7 158 int main() {
syundo0730 0:8d2c753a96e7 159 setup();
syundo0730 0:8d2c753a96e7 160 while(1) {
syundo0730 0:8d2c753a96e7 161 loop();
syundo0730 0:8d2c753a96e7 162 }
syundo0730 0:8d2c753a96e7 163 }
syundo0730 0:8d2c753a96e7 164
syundo0730 0:8d2c753a96e7 165 // ================================================================
syundo0730 0:8d2c753a96e7 166 // === INITIAL SETUP ===
syundo0730 0:8d2c753a96e7 167 // ================================================================
syundo0730 0:8d2c753a96e7 168
syundo0730 0:8d2c753a96e7 169 void setup() {
syundo0730 0:8d2c753a96e7 170 // join I2C bus (I2Cdev library doesn't do this automatically)
syundo0730 0:8d2c753a96e7 171 //Wire.begin();
syundo0730 0:8d2c753a96e7 172
syundo0730 0:8d2c753a96e7 173 // initialize serial communication
syundo0730 0:8d2c753a96e7 174 // (115200 chosen because it is required for Teapot Demo output, but it's
syundo0730 0:8d2c753a96e7 175 // really up to you depending on your project)
syundo0730 0:8d2c753a96e7 176 //Serial.begin(115200);
syundo0730 0:8d2c753a96e7 177 //while (!Serial); // wait for Leonardo enumeration, others continue immediately
syundo0730 0:8d2c753a96e7 178
syundo0730 0:8d2c753a96e7 179 // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
syundo0730 0:8d2c753a96e7 180 // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
syundo0730 0:8d2c753a96e7 181 // the baud timing being too misaligned with processor ticks. You must use
syundo0730 0:8d2c753a96e7 182 // 38400 or slower in these cases, or use some kind of external separate
syundo0730 0:8d2c753a96e7 183 // crystal solution for the UART timer.
syundo0730 0:8d2c753a96e7 184
syundo0730 0:8d2c753a96e7 185 // initialize device
syundo0730 0:8d2c753a96e7 186 //Serial.println(F("Initializing I2C devices..."));
syundo0730 0:8d2c753a96e7 187 //pc.baud(921600);
syundo0730 0:8d2c753a96e7 188 pc.printf("Initializing I2C devices...\r\n");
syundo0730 0:8d2c753a96e7 189 mpu.initialize();
syundo0730 0:8d2c753a96e7 190
syundo0730 0:8d2c753a96e7 191 // verify connection
syundo0730 0:8d2c753a96e7 192 //Serial.println(F("Testing device connections..."));
syundo0730 0:8d2c753a96e7 193 pc.printf("Testing device connections...\r\n");
syundo0730 0:8d2c753a96e7 194 if (mpu.testConnection()) pc.printf("MPU6050 connection successful\r\n");
syundo0730 0:8d2c753a96e7 195 else pc.printf("MPU6050 connection failed\r\n");
syundo0730 0:8d2c753a96e7 196 //Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
syundo0730 0:8d2c753a96e7 197
syundo0730 0:8d2c753a96e7 198 // wait for ready
syundo0730 0:8d2c753a96e7 199 //Serial.println(F("\nSend any character to begin DMP programming and demo: "));
syundo0730 0:8d2c753a96e7 200 //while (Serial.available() && Serial.read()); // empty buffer
syundo0730 0:8d2c753a96e7 201 //while (!Serial.available()); // wait for data
syundo0730 0:8d2c753a96e7 202 //while (Serial.available() && Serial.read()); // empty buffer again
syundo0730 0:8d2c753a96e7 203 wait(2);// Added for test. TODO: Remove this if test goes well.
syundo0730 0:8d2c753a96e7 204
syundo0730 0:8d2c753a96e7 205 // load and configure the DMP
syundo0730 0:8d2c753a96e7 206 //Serial.println(F("Initializing DMP..."));
syundo0730 0:8d2c753a96e7 207 pc.printf("Initializing DMP...\r\n");
syundo0730 0:8d2c753a96e7 208 devStatus = mpu.dmpInitialize();
syundo0730 0:8d2c753a96e7 209
syundo0730 0:8d2c753a96e7 210 // make sure it worked (returns 0 if so)
syundo0730 0:8d2c753a96e7 211 if (devStatus == 0) {
syundo0730 0:8d2c753a96e7 212 // turn on the DMP, now that it's ready
syundo0730 0:8d2c753a96e7 213 //Serial.println(F("Enabling DMP..."));
syundo0730 0:8d2c753a96e7 214 pc.printf("Enabling DMP...\r\n");
syundo0730 0:8d2c753a96e7 215 mpu.setDMPEnabled(true);
syundo0730 0:8d2c753a96e7 216
syundo0730 0:8d2c753a96e7 217 // enable Arduino interrupt detection
syundo0730 0:8d2c753a96e7 218 //Serial.println(F("Enabling interrupt detection..."));
syundo0730 0:8d2c753a96e7 219 pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n");
syundo0730 0:8d2c753a96e7 220 checkpin.rise(&dmpDataReady);
syundo0730 0:8d2c753a96e7 221
syundo0730 0:8d2c753a96e7 222 mpuIntStatus = mpu.getIntStatus();
syundo0730 0:8d2c753a96e7 223
syundo0730 0:8d2c753a96e7 224 // set our DMP Ready flag so the main loop() function knows it's okay to use it
syundo0730 0:8d2c753a96e7 225 //Serial.println(F("DMP ready! Waiting for first interrupt..."));
syundo0730 0:8d2c753a96e7 226 pc.printf("DMP ready! Waiting for first interrupt...\r\n");
syundo0730 0:8d2c753a96e7 227 dmpReady = true;
syundo0730 0:8d2c753a96e7 228
syundo0730 0:8d2c753a96e7 229 // get expected DMP packet size for later comparison
syundo0730 0:8d2c753a96e7 230 packetSize = mpu.dmpGetFIFOPacketSize();
syundo0730 0:8d2c753a96e7 231 } else {
syundo0730 0:8d2c753a96e7 232 // ERROR!
syundo0730 0:8d2c753a96e7 233 // 1 = initial memory load failed
syundo0730 0:8d2c753a96e7 234 // 2 = DMP configuration updates failed
syundo0730 0:8d2c753a96e7 235 // (if it's going to break, usually the code will be 1)
syundo0730 0:8d2c753a96e7 236 //Serial.print(F("DMP Initialization failed (code "));
syundo0730 0:8d2c753a96e7 237 //Serial.print(devStatus);
syundo0730 0:8d2c753a96e7 238 //Serial.println(F(")"));
syundo0730 0:8d2c753a96e7 239 pc.printf("DDMP Initialization failed (code ");
syundo0730 0:8d2c753a96e7 240 pc.printf("%d", devStatus);
syundo0730 0:8d2c753a96e7 241 pc.printf(")\r\n");
syundo0730 0:8d2c753a96e7 242 }
syundo0730 0:8d2c753a96e7 243
syundo0730 0:8d2c753a96e7 244 }
syundo0730 0:8d2c753a96e7 245
syundo0730 0:8d2c753a96e7 246
syundo0730 0:8d2c753a96e7 247
syundo0730 0:8d2c753a96e7 248 // ================================================================
syundo0730 0:8d2c753a96e7 249 // === MAIN PROGRAM LOOP ===
syundo0730 0:8d2c753a96e7 250 // ================================================================
syundo0730 0:8d2c753a96e7 251
syundo0730 0:8d2c753a96e7 252 void loop() {
syundo0730 0:8d2c753a96e7 253 // if programming failed, don't try to do anything
syundo0730 0:8d2c753a96e7 254 if (!dmpReady) return;
syundo0730 0:8d2c753a96e7 255
syundo0730 0:8d2c753a96e7 256 mpuInterrupt = true;
syundo0730 0:8d2c753a96e7 257 // wait for MPU interrupt or extra packet(s) available
syundo0730 0:8d2c753a96e7 258 while (!mpuInterrupt && fifoCount < packetSize) {
syundo0730 0:8d2c753a96e7 259 // other program behavior stuff here
syundo0730 0:8d2c753a96e7 260 // .
syundo0730 0:8d2c753a96e7 261 // .
syundo0730 0:8d2c753a96e7 262 // .
syundo0730 0:8d2c753a96e7 263 // if you are really paranoid you can frequently test in between other
syundo0730 0:8d2c753a96e7 264 // stuff to see if mpuInterrupt is true, and if so, "break;" from the
syundo0730 0:8d2c753a96e7 265 // while() loop to immediately process the MPU data
syundo0730 0:8d2c753a96e7 266 // .
syundo0730 0:8d2c753a96e7 267 // .
syundo0730 0:8d2c753a96e7 268 // .
syundo0730 0:8d2c753a96e7 269 }
syundo0730 0:8d2c753a96e7 270
syundo0730 0:8d2c753a96e7 271 // reset interrupt flag and get INT_STATUS byte
syundo0730 0:8d2c753a96e7 272 mpuInterrupt = false;
syundo0730 0:8d2c753a96e7 273 mpuIntStatus = mpu.getIntStatus();
syundo0730 0:8d2c753a96e7 274
syundo0730 0:8d2c753a96e7 275 // get current FIFO count
syundo0730 0:8d2c753a96e7 276 fifoCount = mpu.getFIFOCount();
syundo0730 0:8d2c753a96e7 277
syundo0730 0:8d2c753a96e7 278 // check for overflow (this should never happen unless our code is too inefficient)
syundo0730 0:8d2c753a96e7 279 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
syundo0730 0:8d2c753a96e7 280 // reset so we can continue cleanly
syundo0730 0:8d2c753a96e7 281 mpu.resetFIFO();
syundo0730 0:8d2c753a96e7 282 //Serial.println(F("FIFO overflow!"));
syundo0730 0:8d2c753a96e7 283
syundo0730 0:8d2c753a96e7 284 // otherwise, check for DMP data ready interrupt (this should happen frequently)
syundo0730 0:8d2c753a96e7 285 } else if (mpuIntStatus & 0x02) {
syundo0730 0:8d2c753a96e7 286 // wait for correct available data length, should be a VERY short wait
syundo0730 0:8d2c753a96e7 287 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
syundo0730 0:8d2c753a96e7 288
syundo0730 0:8d2c753a96e7 289 // read a packet from FIFO
syundo0730 0:8d2c753a96e7 290 mpu.getFIFOBytes(fifoBuffer, packetSize);
syundo0730 0:8d2c753a96e7 291
syundo0730 0:8d2c753a96e7 292 // track FIFO count here in case there is > 1 packet available
syundo0730 0:8d2c753a96e7 293 // (this lets us immediately read more without waiting for an interrupt)
syundo0730 0:8d2c753a96e7 294 fifoCount -= packetSize;
syundo0730 0:8d2c753a96e7 295
syundo0730 0:8d2c753a96e7 296 #ifdef OUTPUT_READABLE_QUATERNION
syundo0730 0:8d2c753a96e7 297 // display quaternion values in easy matrix form: w x y z
syundo0730 0:8d2c753a96e7 298 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 0:8d2c753a96e7 299 //Serial.print("quat\t");
syundo0730 0:8d2c753a96e7 300 //Serial.print(q.w);
syundo0730 0:8d2c753a96e7 301 //Serial.print("\t");
syundo0730 0:8d2c753a96e7 302 //Serial.print(q.x);
syundo0730 0:8d2c753a96e7 303 //Serial.print("\t");
syundo0730 0:8d2c753a96e7 304 //Serial.print(q.y);
syundo0730 0:8d2c753a96e7 305 //Serial.print("\t");
syundo0730 0:8d2c753a96e7 306 //Serial.println(q.z);
syundo0730 0:8d2c753a96e7 307 #endif
syundo0730 0:8d2c753a96e7 308
syundo0730 0:8d2c753a96e7 309 #ifdef OUTPUT_READABLE_EULER
syundo0730 0:8d2c753a96e7 310 // display Euler angles in degrees
syundo0730 0:8d2c753a96e7 311 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 0:8d2c753a96e7 312 mpu.dmpGetEuler(euler, &q);
syundo0730 0:8d2c753a96e7 313 //Serial.print("euler\t");
syundo0730 0:8d2c753a96e7 314 //Serial.print(euler[0] * 180/M_PI);
syundo0730 0:8d2c753a96e7 315 //Serial.print("\t");
syundo0730 0:8d2c753a96e7 316 //Serial.print(euler[1] * 180/M_PI);
syundo0730 0:8d2c753a96e7 317 //Serial.print("\t");
syundo0730 0:8d2c753a96e7 318 //Serial.println(euler[2] * 180/M_PI);
syundo0730 0:8d2c753a96e7 319 printf("euler\t");
syundo0730 0:8d2c753a96e7 320 printf("%f\t", euler[0] * 180/M_PI);
syundo0730 0:8d2c753a96e7 321 printf("%f\t", euler[1] * 180/M_PI);
syundo0730 0:8d2c753a96e7 322 printf("%f\t\r\n", euler[2] * 180/M_PI);
syundo0730 0:8d2c753a96e7 323 #endif
syundo0730 0:8d2c753a96e7 324
syundo0730 0:8d2c753a96e7 325 #ifdef OUTPUT_READABLE_YAWPITCHROLL
syundo0730 0:8d2c753a96e7 326 // display Euler angles in degrees
syundo0730 0:8d2c753a96e7 327 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 0:8d2c753a96e7 328 mpu.dmpGetGravity(&gravity, &q);
syundo0730 0:8d2c753a96e7 329 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
syundo0730 0:8d2c753a96e7 330 //Serial.print("ypr\t");
syundo0730 0:8d2c753a96e7 331 //Serial.print(ypr[0] * 180/M_PI);
syundo0730 0:8d2c753a96e7 332 //Serial.print("\t");
syundo0730 0:8d2c753a96e7 333 //Serial.print(ypr[1] * 180/M_PI);
syundo0730 0:8d2c753a96e7 334 //Serial.print("\t");
syundo0730 0:8d2c753a96e7 335 //Serial.println(ypr[2] * 180/M_PI);
syundo0730 0:8d2c753a96e7 336 printf("ypr\t");
syundo0730 0:8d2c753a96e7 337 printf("%f\t", ypr[0] * 180/M_PI);
syundo0730 0:8d2c753a96e7 338 printf("%f\t", ypr[1] * 180/M_PI);
syundo0730 0:8d2c753a96e7 339 printf("%f\t\r\n", ypr[2] * 180/M_PI);
syundo0730 0:8d2c753a96e7 340 #endif
syundo0730 0:8d2c753a96e7 341
syundo0730 0:8d2c753a96e7 342 #ifdef OUTPUT_READABLE_REALACCEL
syundo0730 0:8d2c753a96e7 343 // display real acceleration, adjusted to remove gravity
syundo0730 0:8d2c753a96e7 344 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 0:8d2c753a96e7 345 mpu.dmpGetAccel(&aa, fifoBuffer);
syundo0730 0:8d2c753a96e7 346 mpu.dmpGetGravity(&gravity, &q);
syundo0730 0:8d2c753a96e7 347 mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
syundo0730 0:8d2c753a96e7 348 //Serial.print("areal\t");
syundo0730 0:8d2c753a96e7 349 //Serial.print(aaReal.x);
syundo0730 0:8d2c753a96e7 350 //Serial.print("\t");
syundo0730 0:8d2c753a96e7 351 //Serial.print(aaReal.y);
syundo0730 0:8d2c753a96e7 352 //Serial.print("\t");
syundo0730 0:8d2c753a96e7 353 //Serial.println(aaReal.z);
syundo0730 0:8d2c753a96e7 354 #endif
syundo0730 0:8d2c753a96e7 355
syundo0730 0:8d2c753a96e7 356 #ifdef OUTPUT_READABLE_WORLDACCEL
syundo0730 0:8d2c753a96e7 357 // display initial world-frame acceleration, adjusted to remove gravity
syundo0730 0:8d2c753a96e7 358 // and rotated based on known orientation from quaternion
syundo0730 0:8d2c753a96e7 359 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 0:8d2c753a96e7 360 mpu.dmpGetAccel(&aa, fifoBuffer);
syundo0730 0:8d2c753a96e7 361 mpu.dmpGetGravity(&gravity, &q);
syundo0730 0:8d2c753a96e7 362 mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
syundo0730 0:8d2c753a96e7 363 //Serial.print("aworld\t");
syundo0730 0:8d2c753a96e7 364 //Serial.print(aaWorld.x);
syundo0730 0:8d2c753a96e7 365 //Serial.print("\t");
syundo0730 0:8d2c753a96e7 366 //Serial.print(aaWorld.y);
syundo0730 0:8d2c753a96e7 367 //Serial.print("\t");
syundo0730 0:8d2c753a96e7 368 //Serial.println(aaWorld.z);
syundo0730 0:8d2c753a96e7 369 #endif
syundo0730 0:8d2c753a96e7 370
syundo0730 0:8d2c753a96e7 371 #ifdef OUTPUT_TEAPOT
syundo0730 0:8d2c753a96e7 372 // display quaternion values in InvenSense Teapot demo format:
syundo0730 0:8d2c753a96e7 373 teapotPacket[2] = fifoBuffer[0];
syundo0730 0:8d2c753a96e7 374 teapotPacket[3] = fifoBuffer[1];
syundo0730 0:8d2c753a96e7 375 teapotPacket[4] = fifoBuffer[4];
syundo0730 0:8d2c753a96e7 376 teapotPacket[5] = fifoBuffer[5];
syundo0730 0:8d2c753a96e7 377 teapotPacket[6] = fifoBuffer[8];
syundo0730 0:8d2c753a96e7 378 teapotPacket[7] = fifoBuffer[9];
syundo0730 0:8d2c753a96e7 379 teapotPacket[8] = fifoBuffer[12];
syundo0730 0:8d2c753a96e7 380 teapotPacket[9] = fifoBuffer[13];
syundo0730 0:8d2c753a96e7 381 //Serial.write(teapotPacket, 14);
syundo0730 0:8d2c753a96e7 382 teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
syundo0730 0:8d2c753a96e7 383 #endif
syundo0730 0:8d2c753a96e7 384
syundo0730 0:8d2c753a96e7 385 // blink LED to indicate activity
syundo0730 0:8d2c753a96e7 386 //blinkState = !blinkState;
syundo0730 0:8d2c753a96e7 387 //digitalWrite(LED_PIN, blinkState);
syundo0730 0:8d2c753a96e7 388 led1 = !led1;
syundo0730 0:8d2c753a96e7 389 }
syundo0730 0:8d2c753a96e7 390 }