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@1:ec0a08108442, 2013-11-23 (annotated)

Committer:
syundo0730
Date:
Sat Nov 23 16:47:59 2013 +0000
Revision:
1:ec0a08108442
Parent:
0:8d2c753a96e7
Child:
2:42c4f3a7813f
comments were added and modified

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 1:ec0a08108442 57
syundo0730 0:8d2c753a96e7 58 MPU6050 mpu;
syundo0730 0:8d2c753a96e7 59
syundo0730 0:8d2c753a96e7 60 /* =========================================================================
syundo0730 0:8d2c753a96e7 61 NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
syundo0730 0:8d2c753a96e7 62 depends on the MPU-6050's INT pin being connected to the Arduino's
syundo0730 0:8d2c753a96e7 63 external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
syundo0730 0:8d2c753a96e7 64 digital I/O pin 2.
syundo0730 0:8d2c753a96e7 65 * ========================================================================= */
syundo0730 0:8d2c753a96e7 66
syundo0730 0:8d2c753a96e7 67 /* =========================================================================
syundo0730 0:8d2c753a96e7 68 NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
syundo0730 0:8d2c753a96e7 69 when using Serial.write(buf, len). The Teapot output uses this method.
syundo0730 0:8d2c753a96e7 70 The solution requires a modification to the Arduino USBAPI.h file, which
syundo0730 0:8d2c753a96e7 71 is fortunately simple, but annoying. This will be fixed in the next IDE
syundo0730 0:8d2c753a96e7 72 release. For more info, see these links:
syundo0730 0:8d2c753a96e7 73
syundo0730 0:8d2c753a96e7 74 http://arduino.cc/forum/index.php/topic,109987.0.html
syundo0730 0:8d2c753a96e7 75 http://code.google.com/p/arduino/issues/detail?id=958
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 1:ec0a08108442 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 // MPU control/status vars
syundo0730 0:8d2c753a96e7 116 bool dmpReady = false; // set true if DMP init was successful
syundo0730 0:8d2c753a96e7 117 uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
syundo0730 0:8d2c753a96e7 118 uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
syundo0730 0:8d2c753a96e7 119 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
syundo0730 0:8d2c753a96e7 120 uint16_t fifoCount; // count of all bytes currently in FIFO
syundo0730 0:8d2c753a96e7 121 uint8_t fifoBuffer[64]; // FIFO storage buffer
syundo0730 0:8d2c753a96e7 122
syundo0730 0:8d2c753a96e7 123 // orientation/motion vars
syundo0730 0:8d2c753a96e7 124 Quaternion q; // [w, x, y, z] quaternion container
syundo0730 0:8d2c753a96e7 125 VectorInt16 aa; // [x, y, z] accel sensor measurements
syundo0730 0:8d2c753a96e7 126 VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
syundo0730 0:8d2c753a96e7 127 VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
syundo0730 0:8d2c753a96e7 128 VectorFloat gravity; // [x, y, z] gravity vector
syundo0730 0:8d2c753a96e7 129 float euler[3]; // [psi, theta, phi] Euler angle container
syundo0730 0:8d2c753a96e7 130 float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
syundo0730 0:8d2c753a96e7 131
syundo0730 0:8d2c753a96e7 132 // packet structure for InvenSense teapot demo
syundo0730 0:8d2c753a96e7 133 uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
syundo0730 0:8d2c753a96e7 134
syundo0730 0:8d2c753a96e7 135 DigitalOut led1(LED1);
syundo0730 0:8d2c753a96e7 136 InterruptIn checkpin(p29);
syundo0730 0:8d2c753a96e7 137 Serial pc(USBTX, USBRX);
syundo0730 0:8d2c753a96e7 138
syundo0730 0:8d2c753a96e7 139 // ================================================================
syundo0730 0:8d2c753a96e7 140 // === INTERRUPT DETECTION ROUTINE ===
syundo0730 0:8d2c753a96e7 141 // ================================================================
syundo0730 0:8d2c753a96e7 142
syundo0730 0:8d2c753a96e7 143 volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
syundo0730 0:8d2c753a96e7 144 void dmpDataReady() {
syundo0730 0:8d2c753a96e7 145 mpuInterrupt = true;
syundo0730 0:8d2c753a96e7 146 }
syundo0730 0:8d2c753a96e7 147
syundo0730 0:8d2c753a96e7 148 void setup();
syundo0730 0:8d2c753a96e7 149 void loop();
syundo0730 0:8d2c753a96e7 150
syundo0730 0:8d2c753a96e7 151 int main() {
syundo0730 0:8d2c753a96e7 152 setup();
syundo0730 0:8d2c753a96e7 153 while(1) {
syundo0730 0:8d2c753a96e7 154 loop();
syundo0730 0:8d2c753a96e7 155 }
syundo0730 0:8d2c753a96e7 156 }
syundo0730 0:8d2c753a96e7 157
syundo0730 0:8d2c753a96e7 158 // ================================================================
syundo0730 0:8d2c753a96e7 159 // === INITIAL SETUP ===
syundo0730 0:8d2c753a96e7 160 // ================================================================
syundo0730 0:8d2c753a96e7 161
syundo0730 0:8d2c753a96e7 162 void setup() {
syundo0730 0:8d2c753a96e7 163 // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
syundo0730 0:8d2c753a96e7 164 // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
syundo0730 0:8d2c753a96e7 165 // the baud timing being too misaligned with processor ticks. You must use
syundo0730 0:8d2c753a96e7 166 // 38400 or slower in these cases, or use some kind of external separate
syundo0730 0:8d2c753a96e7 167 // crystal solution for the UART timer.
syundo0730 0:8d2c753a96e7 168
syundo0730 0:8d2c753a96e7 169 // initialize device
syundo0730 0:8d2c753a96e7 170 pc.printf("Initializing I2C devices...\r\n");
syundo0730 0:8d2c753a96e7 171 mpu.initialize();
syundo0730 0:8d2c753a96e7 172
syundo0730 0:8d2c753a96e7 173 // verify connection
syundo0730 0:8d2c753a96e7 174 pc.printf("Testing device connections...\r\n");
syundo0730 0:8d2c753a96e7 175 if (mpu.testConnection()) pc.printf("MPU6050 connection successful\r\n");
syundo0730 0:8d2c753a96e7 176 else pc.printf("MPU6050 connection failed\r\n");
syundo0730 0:8d2c753a96e7 177
syundo0730 0:8d2c753a96e7 178 // wait for ready
syundo0730 0:8d2c753a96e7 179 //Serial.println(F("\nSend any character to begin DMP programming and demo: "));
syundo0730 0:8d2c753a96e7 180 //while (Serial.available() && Serial.read()); // empty buffer
syundo0730 0:8d2c753a96e7 181 //while (!Serial.available()); // wait for data
syundo0730 0:8d2c753a96e7 182 //while (Serial.available() && Serial.read()); // empty buffer again
syundo0730 0:8d2c753a96e7 183
syundo0730 0:8d2c753a96e7 184 // load and configure the DMP
syundo0730 0:8d2c753a96e7 185 pc.printf("Initializing DMP...\r\n");
syundo0730 0:8d2c753a96e7 186 devStatus = mpu.dmpInitialize();
syundo0730 0:8d2c753a96e7 187
syundo0730 0:8d2c753a96e7 188 // make sure it worked (returns 0 if so)
syundo0730 0:8d2c753a96e7 189 if (devStatus == 0) {
syundo0730 0:8d2c753a96e7 190 // turn on the DMP, now that it's ready
syundo0730 0:8d2c753a96e7 191 pc.printf("Enabling DMP...\r\n");
syundo0730 0:8d2c753a96e7 192 mpu.setDMPEnabled(true);
syundo0730 0:8d2c753a96e7 193
syundo0730 0:8d2c753a96e7 194 // enable Arduino interrupt detection
syundo0730 0:8d2c753a96e7 195 pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n");
syundo0730 0:8d2c753a96e7 196 checkpin.rise(&dmpDataReady);
syundo0730 0:8d2c753a96e7 197
syundo0730 0:8d2c753a96e7 198 mpuIntStatus = mpu.getIntStatus();
syundo0730 0:8d2c753a96e7 199
syundo0730 0:8d2c753a96e7 200 // set our DMP Ready flag so the main loop() function knows it's okay to use it
syundo0730 0:8d2c753a96e7 201 pc.printf("DMP ready! Waiting for first interrupt...\r\n");
syundo0730 0:8d2c753a96e7 202 dmpReady = true;
syundo0730 0:8d2c753a96e7 203
syundo0730 0:8d2c753a96e7 204 // get expected DMP packet size for later comparison
syundo0730 0:8d2c753a96e7 205 packetSize = mpu.dmpGetFIFOPacketSize();
syundo0730 0:8d2c753a96e7 206 } else {
syundo0730 0:8d2c753a96e7 207 // ERROR!
syundo0730 0:8d2c753a96e7 208 // 1 = initial memory load failed
syundo0730 0:8d2c753a96e7 209 // 2 = DMP configuration updates failed
syundo0730 0:8d2c753a96e7 210 // (if it's going to break, usually the code will be 1)
syundo0730 1:ec0a08108442 211
syundo0730 0:8d2c753a96e7 212 pc.printf("DDMP Initialization failed (code ");
syundo0730 0:8d2c753a96e7 213 pc.printf("%d", devStatus);
syundo0730 0:8d2c753a96e7 214 pc.printf(")\r\n");
syundo0730 0:8d2c753a96e7 215 }
syundo0730 0:8d2c753a96e7 216
syundo0730 0:8d2c753a96e7 217 }
syundo0730 0:8d2c753a96e7 218
syundo0730 0:8d2c753a96e7 219
syundo0730 0:8d2c753a96e7 220
syundo0730 0:8d2c753a96e7 221 // ================================================================
syundo0730 0:8d2c753a96e7 222 // === MAIN PROGRAM LOOP ===
syundo0730 0:8d2c753a96e7 223 // ================================================================
syundo0730 0:8d2c753a96e7 224
syundo0730 0:8d2c753a96e7 225 void loop() {
syundo0730 0:8d2c753a96e7 226 // if programming failed, don't try to do anything
syundo0730 0:8d2c753a96e7 227 if (!dmpReady) return;
syundo0730 0:8d2c753a96e7 228
syundo0730 0:8d2c753a96e7 229 // wait for MPU interrupt or extra packet(s) available
syundo0730 0:8d2c753a96e7 230 while (!mpuInterrupt && fifoCount < packetSize) {
syundo0730 0:8d2c753a96e7 231 // other program behavior stuff here
syundo0730 0:8d2c753a96e7 232 // .
syundo0730 0:8d2c753a96e7 233 // .
syundo0730 0:8d2c753a96e7 234 // .
syundo0730 0:8d2c753a96e7 235 // if you are really paranoid you can frequently test in between other
syundo0730 0:8d2c753a96e7 236 // stuff to see if mpuInterrupt is true, and if so, "break;" from the
syundo0730 0:8d2c753a96e7 237 // while() loop to immediately process the MPU data
syundo0730 0:8d2c753a96e7 238 // .
syundo0730 0:8d2c753a96e7 239 // .
syundo0730 0:8d2c753a96e7 240 // .
syundo0730 0:8d2c753a96e7 241 }
syundo0730 0:8d2c753a96e7 242
syundo0730 0:8d2c753a96e7 243 // reset interrupt flag and get INT_STATUS byte
syundo0730 0:8d2c753a96e7 244 mpuInterrupt = false;
syundo0730 0:8d2c753a96e7 245 mpuIntStatus = mpu.getIntStatus();
syundo0730 0:8d2c753a96e7 246
syundo0730 0:8d2c753a96e7 247 // get current FIFO count
syundo0730 0:8d2c753a96e7 248 fifoCount = mpu.getFIFOCount();
syundo0730 0:8d2c753a96e7 249
syundo0730 0:8d2c753a96e7 250 // check for overflow (this should never happen unless our code is too inefficient)
syundo0730 0:8d2c753a96e7 251 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
syundo0730 0:8d2c753a96e7 252 // reset so we can continue cleanly
syundo0730 0:8d2c753a96e7 253 mpu.resetFIFO();
syundo0730 0:8d2c753a96e7 254 //Serial.println(F("FIFO overflow!"));
syundo0730 0:8d2c753a96e7 255
syundo0730 0:8d2c753a96e7 256 // otherwise, check for DMP data ready interrupt (this should happen frequently)
syundo0730 0:8d2c753a96e7 257 } else if (mpuIntStatus & 0x02) {
syundo0730 0:8d2c753a96e7 258 // wait for correct available data length, should be a VERY short wait
syundo0730 0:8d2c753a96e7 259 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
syundo0730 0:8d2c753a96e7 260
syundo0730 0:8d2c753a96e7 261 // read a packet from FIFO
syundo0730 0:8d2c753a96e7 262 mpu.getFIFOBytes(fifoBuffer, packetSize);
syundo0730 0:8d2c753a96e7 263
syundo0730 0:8d2c753a96e7 264 // track FIFO count here in case there is > 1 packet available
syundo0730 0:8d2c753a96e7 265 // (this lets us immediately read more without waiting for an interrupt)
syundo0730 0:8d2c753a96e7 266 fifoCount -= packetSize;
syundo0730 0:8d2c753a96e7 267
syundo0730 0:8d2c753a96e7 268 #ifdef OUTPUT_READABLE_QUATERNION
syundo0730 0:8d2c753a96e7 269 // display quaternion values in easy matrix form: w x y z
syundo0730 0:8d2c753a96e7 270 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 1:ec0a08108442 271 printf("quat\t");
syundo0730 1:ec0a08108442 272 printf("%f\t", q.w);
syundo0730 1:ec0a08108442 273 printf("%f\t", q.x);
syundo0730 1:ec0a08108442 274 printf("%f\t", q.y);
syundo0730 1:ec0a08108442 275 printf("%f\t\r\n", q.z);
syundo0730 0:8d2c753a96e7 276 #endif
syundo0730 0:8d2c753a96e7 277
syundo0730 0:8d2c753a96e7 278 #ifdef OUTPUT_READABLE_EULER
syundo0730 0:8d2c753a96e7 279 // display Euler angles in degrees
syundo0730 0:8d2c753a96e7 280 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 0:8d2c753a96e7 281 mpu.dmpGetEuler(euler, &q);
syundo0730 0:8d2c753a96e7 282 printf("euler\t");
syundo0730 0:8d2c753a96e7 283 printf("%f\t", euler[0] * 180/M_PI);
syundo0730 0:8d2c753a96e7 284 printf("%f\t", euler[1] * 180/M_PI);
syundo0730 0:8d2c753a96e7 285 printf("%f\t\r\n", euler[2] * 180/M_PI);
syundo0730 0:8d2c753a96e7 286 #endif
syundo0730 0:8d2c753a96e7 287
syundo0730 0:8d2c753a96e7 288 #ifdef OUTPUT_READABLE_YAWPITCHROLL
syundo0730 0:8d2c753a96e7 289 // display Euler angles in degrees
syundo0730 0:8d2c753a96e7 290 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 0:8d2c753a96e7 291 mpu.dmpGetGravity(&gravity, &q);
syundo0730 0:8d2c753a96e7 292 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
syundo0730 0:8d2c753a96e7 293 printf("ypr\t");
syundo0730 0:8d2c753a96e7 294 printf("%f\t", ypr[0] * 180/M_PI);
syundo0730 0:8d2c753a96e7 295 printf("%f\t", ypr[1] * 180/M_PI);
syundo0730 0:8d2c753a96e7 296 printf("%f\t\r\n", ypr[2] * 180/M_PI);
syundo0730 0:8d2c753a96e7 297 #endif
syundo0730 0:8d2c753a96e7 298
syundo0730 0:8d2c753a96e7 299 #ifdef OUTPUT_READABLE_REALACCEL
syundo0730 0:8d2c753a96e7 300 // display real acceleration, adjusted to remove gravity
syundo0730 0:8d2c753a96e7 301 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 0:8d2c753a96e7 302 mpu.dmpGetAccel(&aa, fifoBuffer);
syundo0730 0:8d2c753a96e7 303 mpu.dmpGetGravity(&gravity, &q);
syundo0730 0:8d2c753a96e7 304 mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
syundo0730 1:ec0a08108442 305 printf("areal\t");
syundo0730 1:ec0a08108442 306 printf("%f\t", aaReal.x);
syundo0730 1:ec0a08108442 307 printf("%f\t", aaReal.y);
syundo0730 1:ec0a08108442 308 printf("%f\t\r\n", aaReal.z);
syundo0730 0:8d2c753a96e7 309 #endif
syundo0730 0:8d2c753a96e7 310
syundo0730 0:8d2c753a96e7 311 #ifdef OUTPUT_READABLE_WORLDACCEL
syundo0730 0:8d2c753a96e7 312 // display initial world-frame acceleration, adjusted to remove gravity
syundo0730 0:8d2c753a96e7 313 // and rotated based on known orientation from quaternion
syundo0730 0:8d2c753a96e7 314 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 0:8d2c753a96e7 315 mpu.dmpGetAccel(&aa, fifoBuffer);
syundo0730 0:8d2c753a96e7 316 mpu.dmpGetGravity(&gravity, &q);
syundo0730 0:8d2c753a96e7 317 mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
syundo0730 1:ec0a08108442 318 printf("aworld\t");
syundo0730 1:ec0a08108442 319 printf("%f\t", aaWorld.x);
syundo0730 1:ec0a08108442 320 printf("%f\t", aaWorld.y);
syundo0730 1:ec0a08108442 321 printf("%f\t\r\n", aaWorld.z);
syundo0730 0:8d2c753a96e7 322 #endif
syundo0730 0:8d2c753a96e7 323
syundo0730 0:8d2c753a96e7 324 #ifdef OUTPUT_TEAPOT
syundo0730 0:8d2c753a96e7 325 // display quaternion values in InvenSense Teapot demo format:
syundo0730 0:8d2c753a96e7 326 teapotPacket[2] = fifoBuffer[0];
syundo0730 0:8d2c753a96e7 327 teapotPacket[3] = fifoBuffer[1];
syundo0730 0:8d2c753a96e7 328 teapotPacket[4] = fifoBuffer[4];
syundo0730 0:8d2c753a96e7 329 teapotPacket[5] = fifoBuffer[5];
syundo0730 0:8d2c753a96e7 330 teapotPacket[6] = fifoBuffer[8];
syundo0730 0:8d2c753a96e7 331 teapotPacket[7] = fifoBuffer[9];
syundo0730 0:8d2c753a96e7 332 teapotPacket[8] = fifoBuffer[12];
syundo0730 0:8d2c753a96e7 333 teapotPacket[9] = fifoBuffer[13];
syundo0730 1:ec0a08108442 334 for (int i = 0; i < 14; ++i) {
syundo0730 1:ec0a08108442 335 pc.send(teapotPacket[i]);
syundo0730 1:ec0a08108442 336 }
syundo0730 0:8d2c753a96e7 337 teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
syundo0730 0:8d2c753a96e7 338 #endif
syundo0730 0:8d2c753a96e7 339
syundo0730 0:8d2c753a96e7 340 // blink LED to indicate activity
syundo0730 0:8d2c753a96e7 341 led1 = !led1;
syundo0730 0:8d2c753a96e7 342 }
syundo0730 0:8d2c753a96e7 343 }