Rolf Ziegler / Mbed 2 deprecated MPU6050-DMP_6DOF

Complete MPU6050dmp example working with processing 2.1 teapot code v1.0 Jan. 2014

Dependencies:   mbed

Fork of MPU6050 by Shundo Kishi

main.cpp@6:029b4b8111ef, 2014-01-10 (annotated)

Committer:
rolfz
Date:
Fri Jan 10 17:01:28 2014 +0000
Revision:
6:029b4b8111ef
Complete MBED example with main code, working with Processing v2.1 Teapot example!

Who changed what in which revision?

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