sem kleyman / MPU6050

1

Dependencies:   ArduinoSerial I2Cdev

Fork of MPU6050 by Shundo Kishi

main.cpp@9:338c5b334fa6, 2017-06-15 (annotated)

Committer:
sem40590
Date:
Thu Jun 15 15:10:25 2017 +0000
Revision:
9:338c5b334fa6
Child:
10:d0c43cee874b
1

Who changed what in which revision?

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