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

Examples/MPU6050_DMP6.cpp@2:42c4f3a7813f, 2016-01-31 (annotated)

Committer:
syundo0730
Date:
Sun Jan 31 14:12:13 2016 +0000
Revision:
2:42c4f3a7813f
integrated the example for DMP and raw value

Who changed what in which revision?

UserRevisionLine numberNew contents of line
syundo0730 2:42c4f3a7813f 1 #include "MPU6050_DMP6.h"
syundo0730 2:42c4f3a7813f 2
syundo0730 2:42c4f3a7813f 3 // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
syundo0730 2:42c4f3a7813f 4 // for both classes must be in the include path of your project
syundo0730 2:42c4f3a7813f 5 #include "I2Cdev.h"
syundo0730 2:42c4f3a7813f 6
syundo0730 2:42c4f3a7813f 7 #include "MPU6050_6Axis_MotionApps20.h"
syundo0730 2:42c4f3a7813f 8 //#include "MPU6050.h" // not necessary if using MotionApps include file
syundo0730 2:42c4f3a7813f 9
syundo0730 2:42c4f3a7813f 10 #include "ArduinoSerial.h"
syundo0730 2:42c4f3a7813f 11
syundo0730 2:42c4f3a7813f 12 /* =========================================================================
syundo0730 2:42c4f3a7813f 13 NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
syundo0730 2:42c4f3a7813f 14 depends on the MPU-6050's INT pin being connected to the Arduino's
syundo0730 2:42c4f3a7813f 15 external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
syundo0730 2:42c4f3a7813f 16 digital I/O pin 2.
syundo0730 2:42c4f3a7813f 17 * ========================================================================= */
syundo0730 2:42c4f3a7813f 18
syundo0730 2:42c4f3a7813f 19 // uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
syundo0730 2:42c4f3a7813f 20 // quaternion components in a [w, x, y, z] format (not best for parsing
syundo0730 2:42c4f3a7813f 21 // on a remote host such as Processing or something though)
syundo0730 2:42c4f3a7813f 22 //#define OUTPUT_READABLE_QUATERNION
syundo0730 2:42c4f3a7813f 23
syundo0730 2:42c4f3a7813f 24 // uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
syundo0730 2:42c4f3a7813f 25 // (in degrees) calculated from the quaternions coming from the FIFO.
syundo0730 2:42c4f3a7813f 26 // Note that Euler angles suffer from gimbal lock (for more info, see
syundo0730 2:42c4f3a7813f 27 // http://en.wikipedia.org/wiki/Gimbal_lock)
syundo0730 2:42c4f3a7813f 28 //#define OUTPUT_READABLE_EULER
syundo0730 2:42c4f3a7813f 29
syundo0730 2:42c4f3a7813f 30 // uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
syundo0730 2:42c4f3a7813f 31 // pitch/roll angles (in degrees) calculated from the quaternions coming
syundo0730 2:42c4f3a7813f 32 // from the FIFO. Note this also requires gravity vector calculations.
syundo0730 2:42c4f3a7813f 33 // Also note that yaw/pitch/roll angles suffer from gimbal lock (for
syundo0730 2:42c4f3a7813f 34 // more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
syundo0730 2:42c4f3a7813f 35 #define OUTPUT_READABLE_YAWPITCHROLL
syundo0730 2:42c4f3a7813f 36
syundo0730 2:42c4f3a7813f 37 // uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
syundo0730 2:42c4f3a7813f 38 // components with gravity removed. This acceleration reference frame is
syundo0730 2:42c4f3a7813f 39 // not compensated for orientation, so +X is always +X according to the
syundo0730 2:42c4f3a7813f 40 // sensor, just without the effects of gravity. If you want acceleration
syundo0730 2:42c4f3a7813f 41 // compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
syundo0730 2:42c4f3a7813f 42 //#define OUTPUT_READABLE_REALACCEL
syundo0730 2:42c4f3a7813f 43
syundo0730 2:42c4f3a7813f 44 // uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
syundo0730 2:42c4f3a7813f 45 // components with gravity removed and adjusted for the world frame of
syundo0730 2:42c4f3a7813f 46 // reference (yaw is relative to initial orientation, since no magnetometer
syundo0730 2:42c4f3a7813f 47 // is present in this case). Could be quite handy in some cases.
syundo0730 2:42c4f3a7813f 48 //#define OUTPUT_READABLE_WORLDACCEL
syundo0730 2:42c4f3a7813f 49
syundo0730 2:42c4f3a7813f 50 // uncomment "OUTPUT_TEAPOT" if you want output that matches the
syundo0730 2:42c4f3a7813f 51 // format used for the InvenSense teapot demo
syundo0730 2:42c4f3a7813f 52 //#define OUTPUT_TEAPOT
syundo0730 2:42c4f3a7813f 53
syundo0730 2:42c4f3a7813f 54 #ifndef M_PI
syundo0730 2:42c4f3a7813f 55 #define M_PI 3.14159265358979
syundo0730 2:42c4f3a7813f 56 #endif
syundo0730 2:42c4f3a7813f 57
syundo0730 2:42c4f3a7813f 58 namespace MPU6050DMP6 {
syundo0730 2:42c4f3a7813f 59
syundo0730 2:42c4f3a7813f 60 // class default I2C address is 0x68
syundo0730 2:42c4f3a7813f 61 // specific I2C addresses may be passed as a parameter here
syundo0730 2:42c4f3a7813f 62 // AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
syundo0730 2:42c4f3a7813f 63 // AD0 high = 0x69
syundo0730 2:42c4f3a7813f 64 MPU6050 mpu;
syundo0730 2:42c4f3a7813f 65 //MPU6050 mpu(0x69); // <-- use for AD0 high
syundo0730 2:42c4f3a7813f 66
syundo0730 2:42c4f3a7813f 67
syundo0730 2:42c4f3a7813f 68 // MPU control/status vars
syundo0730 2:42c4f3a7813f 69 bool dmpReady = false; // set true if DMP init was successful
syundo0730 2:42c4f3a7813f 70 uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
syundo0730 2:42c4f3a7813f 71 uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
syundo0730 2:42c4f3a7813f 72 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
syundo0730 2:42c4f3a7813f 73 uint16_t fifoCount; // count of all bytes currently in FIFO
syundo0730 2:42c4f3a7813f 74 uint8_t fifoBuffer[64]; // FIFO storage buffer
syundo0730 2:42c4f3a7813f 75
syundo0730 2:42c4f3a7813f 76 // orientation/motion vars
syundo0730 2:42c4f3a7813f 77 Quaternion q; // [w, x, y, z] quaternion container
syundo0730 2:42c4f3a7813f 78 VectorInt16 aa; // [x, y, z] accel sensor measurements
syundo0730 2:42c4f3a7813f 79 VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
syundo0730 2:42c4f3a7813f 80 VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
syundo0730 2:42c4f3a7813f 81 VectorFloat gravity; // [x, y, z] gravity vector
syundo0730 2:42c4f3a7813f 82 float euler[3]; // [psi, theta, phi] Euler angle container
syundo0730 2:42c4f3a7813f 83 float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
syundo0730 2:42c4f3a7813f 84
syundo0730 2:42c4f3a7813f 85 // packet structure for InvenSense teapot demo
syundo0730 2:42c4f3a7813f 86 uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
syundo0730 2:42c4f3a7813f 87
syundo0730 2:42c4f3a7813f 88 DigitalOut led1(LED1);
syundo0730 2:42c4f3a7813f 89 InterruptIn checkpin(p29);
syundo0730 2:42c4f3a7813f 90 ArduinoSerial arduinoSerial;
syundo0730 2:42c4f3a7813f 91
syundo0730 2:42c4f3a7813f 92
syundo0730 2:42c4f3a7813f 93 // ================================================================
syundo0730 2:42c4f3a7813f 94 // === INTERRUPT DETECTION ROUTINE ===
syundo0730 2:42c4f3a7813f 95 // ================================================================
syundo0730 2:42c4f3a7813f 96
syundo0730 2:42c4f3a7813f 97 volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
syundo0730 2:42c4f3a7813f 98 void dmpDataReady()
syundo0730 2:42c4f3a7813f 99 {
syundo0730 2:42c4f3a7813f 100 mpuInterrupt = true;
syundo0730 2:42c4f3a7813f 101 }
syundo0730 2:42c4f3a7813f 102
syundo0730 2:42c4f3a7813f 103
syundo0730 2:42c4f3a7813f 104
syundo0730 2:42c4f3a7813f 105 // ================================================================
syundo0730 2:42c4f3a7813f 106 // === INITIAL SETUP ===
syundo0730 2:42c4f3a7813f 107 // ================================================================
syundo0730 2:42c4f3a7813f 108
syundo0730 2:42c4f3a7813f 109 void setup()
syundo0730 2:42c4f3a7813f 110 {
syundo0730 2:42c4f3a7813f 111 // initialize arduinoSerial communication
syundo0730 2:42c4f3a7813f 112 // (115200 chosen because it is required for Teapot Demo output, but it's
syundo0730 2:42c4f3a7813f 113 // really up to you depending on your project)
syundo0730 2:42c4f3a7813f 114 arduinoSerial.begin(115200);
syundo0730 2:42c4f3a7813f 115
syundo0730 2:42c4f3a7813f 116 // initialize device
syundo0730 2:42c4f3a7813f 117 arduinoSerial.println(F("Initializing I2C devices..."));
syundo0730 2:42c4f3a7813f 118 mpu.initialize();
syundo0730 2:42c4f3a7813f 119
syundo0730 2:42c4f3a7813f 120 // verify connection
syundo0730 2:42c4f3a7813f 121 arduinoSerial.println(F("Testing device connections..."));
syundo0730 2:42c4f3a7813f 122 arduinoSerial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
syundo0730 2:42c4f3a7813f 123
syundo0730 2:42c4f3a7813f 124 // wait for ready
syundo0730 2:42c4f3a7813f 125 // arduinoSerial.println(F("\nSend any character to begin DMP programming and demo: "));
syundo0730 2:42c4f3a7813f 126 // while (arduinoSerial.available() && arduinoSerial.read()); // empty buffer
syundo0730 2:42c4f3a7813f 127 // while (!arduinoSerial.available()); // wait for data
syundo0730 2:42c4f3a7813f 128 // while (arduinoSerial.available() && arduinoSerial.read()); // empty buffer again
syundo0730 2:42c4f3a7813f 129
syundo0730 2:42c4f3a7813f 130 // load and configure the DMP
syundo0730 2:42c4f3a7813f 131 arduinoSerial.println(F("Initializing DMP..."));
syundo0730 2:42c4f3a7813f 132 devStatus = mpu.dmpInitialize();
syundo0730 2:42c4f3a7813f 133
syundo0730 2:42c4f3a7813f 134 // supply your own gyro offsets here, scaled for min sensitivity
syundo0730 2:42c4f3a7813f 135 mpu.setXGyroOffset(220);
syundo0730 2:42c4f3a7813f 136 mpu.setYGyroOffset(76);
syundo0730 2:42c4f3a7813f 137 mpu.setZGyroOffset(-85);
syundo0730 2:42c4f3a7813f 138 mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
syundo0730 2:42c4f3a7813f 139
syundo0730 2:42c4f3a7813f 140 // make sure it worked (returns 0 if so)
syundo0730 2:42c4f3a7813f 141 if (devStatus == 0) {
syundo0730 2:42c4f3a7813f 142 // turn on the DMP, now that it's ready
syundo0730 2:42c4f3a7813f 143 arduinoSerial.println(F("Enabling DMP..."));
syundo0730 2:42c4f3a7813f 144 mpu.setDMPEnabled(true);
syundo0730 2:42c4f3a7813f 145
syundo0730 2:42c4f3a7813f 146 // enable Arduino interrupt detection
syundo0730 2:42c4f3a7813f 147 arduinoSerial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
syundo0730 2:42c4f3a7813f 148 checkpin.rise(&dmpDataReady);
syundo0730 2:42c4f3a7813f 149 mpuIntStatus = mpu.getIntStatus();
syundo0730 2:42c4f3a7813f 150
syundo0730 2:42c4f3a7813f 151 // set our DMP Ready flag so the main loop() function knows it's okay to use it
syundo0730 2:42c4f3a7813f 152 arduinoSerial.println(F("DMP ready! Waiting for first interrupt..."));
syundo0730 2:42c4f3a7813f 153 dmpReady = true;
syundo0730 2:42c4f3a7813f 154
syundo0730 2:42c4f3a7813f 155 // get expected DMP packet size for later comparison
syundo0730 2:42c4f3a7813f 156 packetSize = mpu.dmpGetFIFOPacketSize();
syundo0730 2:42c4f3a7813f 157 } else {
syundo0730 2:42c4f3a7813f 158 // ERROR!
syundo0730 2:42c4f3a7813f 159 // 1 = initial memory load failed
syundo0730 2:42c4f3a7813f 160 // 2 = DMP configuration updates failed
syundo0730 2:42c4f3a7813f 161 // (if it's going to break, usually the code will be 1)
syundo0730 2:42c4f3a7813f 162 arduinoSerial.print(F("DMP Initialization failed (code "));
syundo0730 2:42c4f3a7813f 163 arduinoSerial.print(devStatus);
syundo0730 2:42c4f3a7813f 164 arduinoSerial.println(F(")"));
syundo0730 2:42c4f3a7813f 165 }
syundo0730 2:42c4f3a7813f 166
syundo0730 2:42c4f3a7813f 167 }
syundo0730 2:42c4f3a7813f 168
syundo0730 2:42c4f3a7813f 169
syundo0730 2:42c4f3a7813f 170
syundo0730 2:42c4f3a7813f 171 // ================================================================
syundo0730 2:42c4f3a7813f 172 // === MAIN PROGRAM LOOP ===
syundo0730 2:42c4f3a7813f 173 // ================================================================
syundo0730 2:42c4f3a7813f 174
syundo0730 2:42c4f3a7813f 175 void loop()
syundo0730 2:42c4f3a7813f 176 {
syundo0730 2:42c4f3a7813f 177 // if programming failed, don't try to do anything
syundo0730 2:42c4f3a7813f 178 if (!dmpReady) return;
syundo0730 2:42c4f3a7813f 179
syundo0730 2:42c4f3a7813f 180 // wait for MPU interrupt or extra packet(s) available
syundo0730 2:42c4f3a7813f 181 while (!mpuInterrupt && fifoCount < packetSize) {
syundo0730 2:42c4f3a7813f 182 // other program behavior stuff here
syundo0730 2:42c4f3a7813f 183 // .
syundo0730 2:42c4f3a7813f 184 // .
syundo0730 2:42c4f3a7813f 185 // .
syundo0730 2:42c4f3a7813f 186 // if you are really paranoid you can frequently test in between other
syundo0730 2:42c4f3a7813f 187 // stuff to see if mpuInterrupt is true, and if so, "break;" from the
syundo0730 2:42c4f3a7813f 188 // while() loop to immediately process the MPU data
syundo0730 2:42c4f3a7813f 189 // .
syundo0730 2:42c4f3a7813f 190 // .
syundo0730 2:42c4f3a7813f 191 // .
syundo0730 2:42c4f3a7813f 192 }
syundo0730 2:42c4f3a7813f 193
syundo0730 2:42c4f3a7813f 194 // reset interrupt flag and get INT_STATUS byte
syundo0730 2:42c4f3a7813f 195 mpuInterrupt = false;
syundo0730 2:42c4f3a7813f 196 mpuIntStatus = mpu.getIntStatus();
syundo0730 2:42c4f3a7813f 197
syundo0730 2:42c4f3a7813f 198 // get current FIFO count
syundo0730 2:42c4f3a7813f 199 fifoCount = mpu.getFIFOCount();
syundo0730 2:42c4f3a7813f 200
syundo0730 2:42c4f3a7813f 201 // check for overflow (this should never happen unless our code is too inefficient)
syundo0730 2:42c4f3a7813f 202 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
syundo0730 2:42c4f3a7813f 203 // reset so we can continue cleanly
syundo0730 2:42c4f3a7813f 204 mpu.resetFIFO();
syundo0730 2:42c4f3a7813f 205 arduinoSerial.println(F("FIFO overflow!"));
syundo0730 2:42c4f3a7813f 206
syundo0730 2:42c4f3a7813f 207 // otherwise, check for DMP data ready interrupt (this should happen frequently)
syundo0730 2:42c4f3a7813f 208 } else if (mpuIntStatus & 0x02) {
syundo0730 2:42c4f3a7813f 209 // wait for correct available data length, should be a VERY short wait
syundo0730 2:42c4f3a7813f 210 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
syundo0730 2:42c4f3a7813f 211
syundo0730 2:42c4f3a7813f 212 // read a packet from FIFO
syundo0730 2:42c4f3a7813f 213 mpu.getFIFOBytes(fifoBuffer, packetSize);
syundo0730 2:42c4f3a7813f 214
syundo0730 2:42c4f3a7813f 215 // track FIFO count here in case there is > 1 packet available
syundo0730 2:42c4f3a7813f 216 // (this lets us immediately read more without waiting for an interrupt)
syundo0730 2:42c4f3a7813f 217 fifoCount -= packetSize;
syundo0730 2:42c4f3a7813f 218
syundo0730 2:42c4f3a7813f 219 #ifdef OUTPUT_READABLE_QUATERNION
syundo0730 2:42c4f3a7813f 220 // display quaternion values in easy matrix form: w x y z
syundo0730 2:42c4f3a7813f 221 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 2:42c4f3a7813f 222 arduinoSerial.print("quat\t");
syundo0730 2:42c4f3a7813f 223 arduinoSerial.print(q.w);
syundo0730 2:42c4f3a7813f 224 arduinoSerial.print("\t");
syundo0730 2:42c4f3a7813f 225 arduinoSerial.print(q.x);
syundo0730 2:42c4f3a7813f 226 arduinoSerial.print("\t");
syundo0730 2:42c4f3a7813f 227 arduinoSerial.print(q.y);
syundo0730 2:42c4f3a7813f 228 arduinoSerial.print("\t");
syundo0730 2:42c4f3a7813f 229 arduinoSerial.println(q.z);
syundo0730 2:42c4f3a7813f 230 #endif
syundo0730 2:42c4f3a7813f 231
syundo0730 2:42c4f3a7813f 232 #ifdef OUTPUT_READABLE_EULER
syundo0730 2:42c4f3a7813f 233 // display Euler angles in degrees
syundo0730 2:42c4f3a7813f 234 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 2:42c4f3a7813f 235 mpu.dmpGetEuler(euler, &q);
syundo0730 2:42c4f3a7813f 236 arduinoSerial.print("euler\t");
syundo0730 2:42c4f3a7813f 237 arduinoSerial.print(euler[0] * 180/M_PI);
syundo0730 2:42c4f3a7813f 238 arduinoSerial.print("\t");
syundo0730 2:42c4f3a7813f 239 arduinoSerial.print(euler[1] * 180/M_PI);
syundo0730 2:42c4f3a7813f 240 arduinoSerial.print("\t");
syundo0730 2:42c4f3a7813f 241 arduinoSerial.println(euler[2] * 180/M_PI);
syundo0730 2:42c4f3a7813f 242 #endif
syundo0730 2:42c4f3a7813f 243
syundo0730 2:42c4f3a7813f 244 #ifdef OUTPUT_READABLE_YAWPITCHROLL
syundo0730 2:42c4f3a7813f 245 // display Euler angles in degrees
syundo0730 2:42c4f3a7813f 246 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 2:42c4f3a7813f 247 mpu.dmpGetGravity(&gravity, &q);
syundo0730 2:42c4f3a7813f 248 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
syundo0730 2:42c4f3a7813f 249 arduinoSerial.print("ypr\t");
syundo0730 2:42c4f3a7813f 250 arduinoSerial.print(ypr[0] * 180/M_PI);
syundo0730 2:42c4f3a7813f 251 arduinoSerial.print("\t");
syundo0730 2:42c4f3a7813f 252 arduinoSerial.print(ypr[1] * 180/M_PI);
syundo0730 2:42c4f3a7813f 253 arduinoSerial.print("\t");
syundo0730 2:42c4f3a7813f 254 arduinoSerial.println(ypr[2] * 180/M_PI);
syundo0730 2:42c4f3a7813f 255 #endif
syundo0730 2:42c4f3a7813f 256
syundo0730 2:42c4f3a7813f 257 #ifdef OUTPUT_READABLE_REALACCEL
syundo0730 2:42c4f3a7813f 258 // display real acceleration, adjusted to remove gravity
syundo0730 2:42c4f3a7813f 259 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 2:42c4f3a7813f 260 mpu.dmpGetAccel(&aa, fifoBuffer);
syundo0730 2:42c4f3a7813f 261 mpu.dmpGetGravity(&gravity, &q);
syundo0730 2:42c4f3a7813f 262 mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
syundo0730 2:42c4f3a7813f 263 arduinoSerial.print("areal\t");
syundo0730 2:42c4f3a7813f 264 arduinoSerial.print(aaReal.x);
syundo0730 2:42c4f3a7813f 265 arduinoSerial.print("\t");
syundo0730 2:42c4f3a7813f 266 arduinoSerial.print(aaReal.y);
syundo0730 2:42c4f3a7813f 267 arduinoSerial.print("\t");
syundo0730 2:42c4f3a7813f 268 arduinoSerial.println(aaReal.z);
syundo0730 2:42c4f3a7813f 269 #endif
syundo0730 2:42c4f3a7813f 270
syundo0730 2:42c4f3a7813f 271 #ifdef OUTPUT_READABLE_WORLDACCEL
syundo0730 2:42c4f3a7813f 272 // display initial world-frame acceleration, adjusted to remove gravity
syundo0730 2:42c4f3a7813f 273 // and rotated based on known orientation from quaternion
syundo0730 2:42c4f3a7813f 274 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 2:42c4f3a7813f 275 mpu.dmpGetAccel(&aa, fifoBuffer);
syundo0730 2:42c4f3a7813f 276 mpu.dmpGetGravity(&gravity, &q);
syundo0730 2:42c4f3a7813f 277 mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
syundo0730 2:42c4f3a7813f 278 mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
syundo0730 2:42c4f3a7813f 279 arduinoSerial.print("aworld\t");
syundo0730 2:42c4f3a7813f 280 arduinoSerial.print(aaWorld.x);
syundo0730 2:42c4f3a7813f 281 arduinoSerial.print("\t");
syundo0730 2:42c4f3a7813f 282 arduinoSerial.print(aaWorld.y);
syundo0730 2:42c4f3a7813f 283 arduinoSerial.print("\t");
syundo0730 2:42c4f3a7813f 284 arduinoSerial.println(aaWorld.z);
syundo0730 2:42c4f3a7813f 285 #endif
syundo0730 2:42c4f3a7813f 286
syundo0730 2:42c4f3a7813f 287 #ifdef OUTPUT_TEAPOT
syundo0730 2:42c4f3a7813f 288 // display quaternion values in InvenSense Teapot demo format:
syundo0730 2:42c4f3a7813f 289 teapotPacket[2] = fifoBuffer[0];
syundo0730 2:42c4f3a7813f 290 teapotPacket[3] = fifoBuffer[1];
syundo0730 2:42c4f3a7813f 291 teapotPacket[4] = fifoBuffer[4];
syundo0730 2:42c4f3a7813f 292 teapotPacket[5] = fifoBuffer[5];
syundo0730 2:42c4f3a7813f 293 teapotPacket[6] = fifoBuffer[8];
syundo0730 2:42c4f3a7813f 294 teapotPacket[7] = fifoBuffer[9];
syundo0730 2:42c4f3a7813f 295 teapotPacket[8] = fifoBuffer[12];
syundo0730 2:42c4f3a7813f 296 teapotPacket[9] = fifoBuffer[13];
syundo0730 2:42c4f3a7813f 297 arduinoSerial.write(teapotPacket, 14);
syundo0730 2:42c4f3a7813f 298 teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
syundo0730 2:42c4f3a7813f 299 #endif
syundo0730 2:42c4f3a7813f 300
syundo0730 2:42c4f3a7813f 301 // blink LED to indicate activity
syundo0730 2:42c4f3a7813f 302 if( led1 == 0 ) led1 = 0;
syundo0730 2:42c4f3a7813f 303 else led1 = 1;
syundo0730 2:42c4f3a7813f 304 }
syundo0730 2:42c4f3a7813f 305 }
syundo0730 2:42c4f3a7813f 306
syundo0730 2:42c4f3a7813f 307 };