Mark Vandermeulen / Mbed 2 deprecated MPU6050-DMP_sample

This code is bad. Don't use this fork as an example. I don't know if this shows up publicly.

Dependencies:   MPU6050-DMP mbed

Fork of MPU6050_Example by Shundo Kishi

main.cpp@2:07e29b6d29da, 2015-03-18 (annotated)

Committer:
majik
Date:
Wed Mar 18 22:21:23 2015 +0000
Revision:
2:07e29b6d29da
Parent:
1:ec0a08108442 
This program successfully sends the DMP data for 2 IMUs.; Only modification required: Lowered the DMP data rate so the FIFO buffer doesn't overflow.

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"
majik 2:07e29b6d29da 51 #include "MPU6051_6Axis_MotionApps20.h"
syundo0730 0:8d2c753a96e7 52 //#include "MPU6050.h" // not necessary if using MotionApps include file
syundo0730 0:8d2c753a96e7 53
syundo0730 0:8d2c753a96e7 54 // class default I2C address is 0x68
syundo0730 0:8d2c753a96e7 55 // specific I2C addresses may be passed as a parameter here
syundo0730 0:8d2c753a96e7 56 // AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
syundo0730 0:8d2c753a96e7 57 // AD0 high = 0x69
syundo0730 1:ec0a08108442 58
syundo0730 0:8d2c753a96e7 59 MPU6050 mpu;
majik 2:07e29b6d29da 60 MPU6051 mpu2;
syundo0730 0:8d2c753a96e7 61
syundo0730 0:8d2c753a96e7 62 /* =========================================================================
syundo0730 0:8d2c753a96e7 63 NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
syundo0730 0:8d2c753a96e7 64 depends on the MPU-6050's INT pin being connected to the Arduino's
syundo0730 0:8d2c753a96e7 65 external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
syundo0730 0:8d2c753a96e7 66 digital I/O pin 2.
syundo0730 0:8d2c753a96e7 67 * ========================================================================= */
syundo0730 0:8d2c753a96e7 68
syundo0730 0:8d2c753a96e7 69 /* =========================================================================
syundo0730 0:8d2c753a96e7 70 NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
syundo0730 0:8d2c753a96e7 71 when using Serial.write(buf, len). The Teapot output uses this method.
syundo0730 0:8d2c753a96e7 72 The solution requires a modification to the Arduino USBAPI.h file, which
syundo0730 0:8d2c753a96e7 73 is fortunately simple, but annoying. This will be fixed in the next IDE
syundo0730 0:8d2c753a96e7 74 release. For more info, see these links:
syundo0730 0:8d2c753a96e7 75
syundo0730 0:8d2c753a96e7 76 http://arduino.cc/forum/index.php/topic,109987.0.html
syundo0730 0:8d2c753a96e7 77 http://code.google.com/p/arduino/issues/detail?id=958
syundo0730 0:8d2c753a96e7 78 * ========================================================================= */
syundo0730 0:8d2c753a96e7 79
syundo0730 0:8d2c753a96e7 80 const float M_PI = 3.14159265;
syundo0730 0:8d2c753a96e7 81
majik 2:07e29b6d29da 82 //uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
syundo0730 0:8d2c753a96e7 83 // quaternion components in a [w, x, y, z] format (not best for parsing
syundo0730 0:8d2c753a96e7 84 // on a remote host such as Processing or something though)
majik 2:07e29b6d29da 85 #define OUTPUT_READABLE_QUATERNION
syundo0730 0:8d2c753a96e7 86
syundo0730 0:8d2c753a96e7 87 // uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
syundo0730 0:8d2c753a96e7 88 // (in degrees) calculated from the quaternions coming from the FIFO.
syundo0730 0:8d2c753a96e7 89 // Note that Euler angles suffer from gimbal lock (for more info, see
syundo0730 0:8d2c753a96e7 90 // http://en.wikipedia.org/wiki/Gimbal_lock)
syundo0730 1:ec0a08108442 91 //#define OUTPUT_READABLE_EULER
syundo0730 0:8d2c753a96e7 92
syundo0730 0:8d2c753a96e7 93 // uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
syundo0730 0:8d2c753a96e7 94 // pitch/roll angles (in degrees) calculated from the quaternions coming
syundo0730 0:8d2c753a96e7 95 // from the FIFO. Note this also requires gravity vector calculations.
syundo0730 0:8d2c753a96e7 96 // Also note that yaw/pitch/roll angles suffer from gimbal lock (for
syundo0730 0:8d2c753a96e7 97 // more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
majik 2:07e29b6d29da 98 #define OUTPUT_READABLE_YAWPITCHROLL
syundo0730 0:8d2c753a96e7 99
syundo0730 0:8d2c753a96e7 100 // uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
syundo0730 0:8d2c753a96e7 101 // components with gravity removed. This acceleration reference frame is
syundo0730 0:8d2c753a96e7 102 // not compensated for orientation, so +X is always +X according to the
syundo0730 0:8d2c753a96e7 103 // sensor, just without the effects of gravity. If you want acceleration
syundo0730 0:8d2c753a96e7 104 // compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
syundo0730 0:8d2c753a96e7 105 //#define OUTPUT_READABLE_REALACCEL
syundo0730 0:8d2c753a96e7 106
syundo0730 0:8d2c753a96e7 107 // uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
syundo0730 0:8d2c753a96e7 108 // components with gravity removed and adjusted for the world frame of
syundo0730 0:8d2c753a96e7 109 // reference (yaw is relative to initial orientation, since no magnetometer
syundo0730 0:8d2c753a96e7 110 // is present in this case). Could be quite handy in some cases.
syundo0730 0:8d2c753a96e7 111 //#define OUTPUT_READABLE_WORLDACCEL
syundo0730 0:8d2c753a96e7 112
syundo0730 0:8d2c753a96e7 113 // uncomment "OUTPUT_TEAPOT" if you want output that matches the
syundo0730 0:8d2c753a96e7 114 // format used for the InvenSense teapot demo
syundo0730 0:8d2c753a96e7 115 //#define OUTPUT_TEAPOT
syundo0730 0:8d2c753a96e7 116
syundo0730 0:8d2c753a96e7 117 // MPU control/status vars
syundo0730 0:8d2c753a96e7 118 bool dmpReady = false; // set true if DMP init was successful
syundo0730 0:8d2c753a96e7 119 uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
syundo0730 0:8d2c753a96e7 120 uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
syundo0730 0:8d2c753a96e7 121 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
syundo0730 0:8d2c753a96e7 122 uint16_t fifoCount; // count of all bytes currently in FIFO
majik 2:07e29b6d29da 123 uint8_t fifoBuffer[128]; // FIFO storage buffer
majik 2:07e29b6d29da 124
majik 2:07e29b6d29da 125 bool dmpReady2 = false; // set true if DMP init was successful
majik 2:07e29b6d29da 126 uint8_t mpuIntStatus2; // holds actual interrupt status byte from MPU
majik 2:07e29b6d29da 127 uint8_t devStatus2; // return status after each device operation (0 = success, !0 = error)
majik 2:07e29b6d29da 128 uint16_t packetSize2; // expected DMP packet size (default is 42 bytes)
majik 2:07e29b6d29da 129 uint16_t fifoCount2; // count of all bytes currently in FIFO
majik 2:07e29b6d29da 130 uint8_t fifoBuffer2[128]; // FIFO storage buffer
syundo0730 0:8d2c753a96e7 131
syundo0730 0:8d2c753a96e7 132 // orientation/motion vars
syundo0730 0:8d2c753a96e7 133 Quaternion q; // [w, x, y, z] quaternion container
syundo0730 0:8d2c753a96e7 134 VectorInt16 aa; // [x, y, z] accel sensor measurements
syundo0730 0:8d2c753a96e7 135 VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
syundo0730 0:8d2c753a96e7 136 VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
syundo0730 0:8d2c753a96e7 137 VectorFloat gravity; // [x, y, z] gravity vector
syundo0730 0:8d2c753a96e7 138 float euler[3]; // [psi, theta, phi] Euler angle container
syundo0730 0:8d2c753a96e7 139 float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
syundo0730 0:8d2c753a96e7 140
majik 2:07e29b6d29da 141
syundo0730 0:8d2c753a96e7 142 // packet structure for InvenSense teapot demo
syundo0730 0:8d2c753a96e7 143 uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
syundo0730 0:8d2c753a96e7 144
majik 2:07e29b6d29da 145 DigitalOut led1(PTE3);
majik 2:07e29b6d29da 146 InterruptIn checkpin(PTD7); //PTD7, PTD5
majik 2:07e29b6d29da 147 InterruptIn checkpin2(PTD5);
majik 2:07e29b6d29da 148 Serial pc(PTA2,PTA1);
majik 2:07e29b6d29da 149 DigitalOut btSwitch(PTE30);
majik 2:07e29b6d29da 150 DigitalOut imuSwitch(PTD6); //PTD6, PTD4
majik 2:07e29b6d29da 151 DigitalOut imu2Switch(PTD4);
majik 2:07e29b6d29da 152 //int counter = 0;
syundo0730 0:8d2c753a96e7 153 // ================================================================
syundo0730 0:8d2c753a96e7 154 // === INTERRUPT DETECTION ROUTINE ===
syundo0730 0:8d2c753a96e7 155 // ================================================================
syundo0730 0:8d2c753a96e7 156
syundo0730 0:8d2c753a96e7 157 volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
syundo0730 0:8d2c753a96e7 158 void dmpDataReady() {
syundo0730 0:8d2c753a96e7 159 mpuInterrupt = true;
syundo0730 0:8d2c753a96e7 160 }
majik 2:07e29b6d29da 161 volatile bool mpuInterrupt2 = false; // indicates whether MPU interrupt pin has gone high
majik 2:07e29b6d29da 162 void dmpDataReady2() {
majik 2:07e29b6d29da 163 mpuInterrupt2 = true;
majik 2:07e29b6d29da 164 }
syundo0730 0:8d2c753a96e7 165
syundo0730 0:8d2c753a96e7 166 void setup();
syundo0730 0:8d2c753a96e7 167 void loop();
majik 2:07e29b6d29da 168 void loop2();
majik 2:07e29b6d29da 169 Timer t;
syundo0730 0:8d2c753a96e7 170 int main() {
syundo0730 0:8d2c753a96e7 171 setup();
majik 2:07e29b6d29da 172 t.start();
majik 2:07e29b6d29da 173 pc.baud(115200);
syundo0730 0:8d2c753a96e7 174 while(1) {
majik 2:07e29b6d29da 175 // pc.printf("Loopy loop\n\r");
majik 2:07e29b6d29da 176 //pc.printf("%f\t",t.read());
syundo0730 0:8d2c753a96e7 177 loop();
majik 2:07e29b6d29da 178 //pc.printf("\t");
majik 2:07e29b6d29da 179 loop2();
majik 2:07e29b6d29da 180 //pc.printf("\n\r");
syundo0730 0:8d2c753a96e7 181 }
syundo0730 0:8d2c753a96e7 182 }
syundo0730 0:8d2c753a96e7 183
syundo0730 0:8d2c753a96e7 184 // ================================================================
syundo0730 0:8d2c753a96e7 185 // === INITIAL SETUP ===
syundo0730 0:8d2c753a96e7 186 // ================================================================
syundo0730 0:8d2c753a96e7 187
syundo0730 0:8d2c753a96e7 188 void setup() {
syundo0730 0:8d2c753a96e7 189 // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
syundo0730 0:8d2c753a96e7 190 // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
syundo0730 0:8d2c753a96e7 191 // the baud timing being too misaligned with processor ticks. You must use
syundo0730 0:8d2c753a96e7 192 // 38400 or slower in these cases, or use some kind of external separate
syundo0730 0:8d2c753a96e7 193 // crystal solution for the UART timer.
majik 2:07e29b6d29da 194 btSwitch = 1;
majik 2:07e29b6d29da 195 imuSwitch = 1;
majik 2:07e29b6d29da 196 imu2Switch = 1;
majik 2:07e29b6d29da 197 wait_ms(200);
majik 2:07e29b6d29da 198 while(!mpu.testConnection()){
majik 2:07e29b6d29da 199 imuSwitch = 0;
majik 2:07e29b6d29da 200 wait_ms(100);
majik 2:07e29b6d29da 201 imuSwitch = 1;
majik 2:07e29b6d29da 202 wait_ms(200);
majik 2:07e29b6d29da 203 led1 = !led1;
majik 2:07e29b6d29da 204 }
majik 2:07e29b6d29da 205 while(!mpu2.testConnection()){
majik 2:07e29b6d29da 206 imu2Switch = 0;
majik 2:07e29b6d29da 207 wait_ms(100);
majik 2:07e29b6d29da 208 imu2Switch = 1;
majik 2:07e29b6d29da 209 wait_ms(200);
majik 2:07e29b6d29da 210 led1 = !led1;
majik 2:07e29b6d29da 211 }
majik 2:07e29b6d29da 212 pc.baud(115200);
syundo0730 0:8d2c753a96e7 213 // initialize device
syundo0730 0:8d2c753a96e7 214 pc.printf("Initializing I2C devices...\r\n");
syundo0730 0:8d2c753a96e7 215 mpu.initialize();
syundo0730 0:8d2c753a96e7 216
syundo0730 0:8d2c753a96e7 217 // verify connection
syundo0730 0:8d2c753a96e7 218 pc.printf("Testing device connections...\r\n");
syundo0730 0:8d2c753a96e7 219 if (mpu.testConnection()) pc.printf("MPU6050 connection successful\r\n");
syundo0730 0:8d2c753a96e7 220 else pc.printf("MPU6050 connection failed\r\n");
syundo0730 0:8d2c753a96e7 221
syundo0730 0:8d2c753a96e7 222 // load and configure the DMP
syundo0730 0:8d2c753a96e7 223 pc.printf("Initializing DMP...\r\n");
syundo0730 0:8d2c753a96e7 224 devStatus = mpu.dmpInitialize();
syundo0730 0:8d2c753a96e7 225
syundo0730 0:8d2c753a96e7 226 // make sure it worked (returns 0 if so)
syundo0730 0:8d2c753a96e7 227 if (devStatus == 0) {
syundo0730 0:8d2c753a96e7 228 // turn on the DMP, now that it's ready
syundo0730 0:8d2c753a96e7 229 pc.printf("Enabling DMP...\r\n");
syundo0730 0:8d2c753a96e7 230 mpu.setDMPEnabled(true);
syundo0730 0:8d2c753a96e7 231
syundo0730 0:8d2c753a96e7 232 // enable Arduino interrupt detection
syundo0730 0:8d2c753a96e7 233 pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n");
syundo0730 0:8d2c753a96e7 234 checkpin.rise(&dmpDataReady);
syundo0730 0:8d2c753a96e7 235
syundo0730 0:8d2c753a96e7 236 mpuIntStatus = mpu.getIntStatus();
syundo0730 0:8d2c753a96e7 237
syundo0730 0:8d2c753a96e7 238 // set our DMP Ready flag so the main loop() function knows it's okay to use it
syundo0730 0:8d2c753a96e7 239 pc.printf("DMP ready! Waiting for first interrupt...\r\n");
syundo0730 0:8d2c753a96e7 240 dmpReady = true;
syundo0730 0:8d2c753a96e7 241
syundo0730 0:8d2c753a96e7 242 // get expected DMP packet size for later comparison
syundo0730 0:8d2c753a96e7 243 packetSize = mpu.dmpGetFIFOPacketSize();
syundo0730 0:8d2c753a96e7 244 } else {
syundo0730 0:8d2c753a96e7 245 // ERROR!
syundo0730 0:8d2c753a96e7 246 // 1 = initial memory load failed
syundo0730 0:8d2c753a96e7 247 // 2 = DMP configuration updates failed
syundo0730 0:8d2c753a96e7 248 // (if it's going to break, usually the code will be 1)
syundo0730 1:ec0a08108442 249
syundo0730 0:8d2c753a96e7 250 pc.printf("DDMP Initialization failed (code ");
syundo0730 0:8d2c753a96e7 251 pc.printf("%d", devStatus);
syundo0730 0:8d2c753a96e7 252 pc.printf(")\r\n");
syundo0730 0:8d2c753a96e7 253 }
majik 2:07e29b6d29da 254 // initialize device
majik 2:07e29b6d29da 255 pc.printf("Initializing I2C devices...\r\n");
majik 2:07e29b6d29da 256 mpu2.initialize();
syundo0730 0:8d2c753a96e7 257
majik 2:07e29b6d29da 258 // verify connection
majik 2:07e29b6d29da 259 pc.printf("Testing device connections...\r\n");
majik 2:07e29b6d29da 260 if (mpu2.testConnection()) pc.printf("MPU6050 connection successful\r\n");
majik 2:07e29b6d29da 261 else pc.printf("MPU6050 connection failed\r\n");
majik 2:07e29b6d29da 262
majik 2:07e29b6d29da 263 // load and configure the DMP
majik 2:07e29b6d29da 264 pc.printf("Initializing DMP...\r\n");
majik 2:07e29b6d29da 265 devStatus2 = mpu2.dmpInitialize();
majik 2:07e29b6d29da 266
majik 2:07e29b6d29da 267 // make sure it worked (returns 0 if so)
majik 2:07e29b6d29da 268 if (devStatus2 == 0) {
majik 2:07e29b6d29da 269 // turn on the DMP, now that it's ready
majik 2:07e29b6d29da 270 pc.printf("Enabling DMP...\r\n");
majik 2:07e29b6d29da 271 mpu2.setDMPEnabled(true);
majik 2:07e29b6d29da 272
majik 2:07e29b6d29da 273 // enable Arduino interrupt detection
majik 2:07e29b6d29da 274 pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n");
majik 2:07e29b6d29da 275 checkpin2.rise(&dmpDataReady2);
majik 2:07e29b6d29da 276
majik 2:07e29b6d29da 277 mpuIntStatus2 = mpu2.getIntStatus();
majik 2:07e29b6d29da 278
majik 2:07e29b6d29da 279 // set our DMP Ready flag so the main loop() function knows it's okay to use it
majik 2:07e29b6d29da 280 pc.printf("DMP ready! Waiting for first interrupt...\r\n");
majik 2:07e29b6d29da 281 dmpReady2 = true;
majik 2:07e29b6d29da 282
majik 2:07e29b6d29da 283 // get expected DMP packet size for later comparison
majik 2:07e29b6d29da 284 packetSize2 = mpu2.dmpGetFIFOPacketSize();
majik 2:07e29b6d29da 285 } else {
majik 2:07e29b6d29da 286 // ERROR!
majik 2:07e29b6d29da 287 // 1 = initial memory load failed
majik 2:07e29b6d29da 288 // 2 = DMP configuration updates failed
majik 2:07e29b6d29da 289 // (if it's going to break, usually the code will be 1)
majik 2:07e29b6d29da 290
majik 2:07e29b6d29da 291 pc.printf("DDMP Initialization failed (code ");
majik 2:07e29b6d29da 292 pc.printf("%d", devStatus2);
majik 2:07e29b6d29da 293 pc.printf(")\r\n");
majik 2:07e29b6d29da 294 }
syundo0730 0:8d2c753a96e7 295 }
syundo0730 0:8d2c753a96e7 296
syundo0730 0:8d2c753a96e7 297
syundo0730 0:8d2c753a96e7 298
syundo0730 0:8d2c753a96e7 299 // ================================================================
syundo0730 0:8d2c753a96e7 300 // === MAIN PROGRAM LOOP ===
syundo0730 0:8d2c753a96e7 301 // ================================================================
syundo0730 0:8d2c753a96e7 302
syundo0730 0:8d2c753a96e7 303 void loop() {
syundo0730 0:8d2c753a96e7 304 // if programming failed, don't try to do anything
syundo0730 0:8d2c753a96e7 305 if (!dmpReady) return;
syundo0730 0:8d2c753a96e7 306
syundo0730 0:8d2c753a96e7 307 // wait for MPU interrupt or extra packet(s) available
syundo0730 0:8d2c753a96e7 308 while (!mpuInterrupt && fifoCount < packetSize) {
syundo0730 0:8d2c753a96e7 309 // other program behavior stuff here
syundo0730 0:8d2c753a96e7 310 // .
syundo0730 0:8d2c753a96e7 311 // .
syundo0730 0:8d2c753a96e7 312 // .
syundo0730 0:8d2c753a96e7 313 // if you are really paranoid you can frequently test in between other
syundo0730 0:8d2c753a96e7 314 // stuff to see if mpuInterrupt is true, and if so, "break;" from the
syundo0730 0:8d2c753a96e7 315 // while() loop to immediately process the MPU data
syundo0730 0:8d2c753a96e7 316 // .
syundo0730 0:8d2c753a96e7 317 // .
syundo0730 0:8d2c753a96e7 318 // .
majik 2:07e29b6d29da 319 //led1 = !led1;
majik 2:07e29b6d29da 320 //wait_ms(100);
syundo0730 0:8d2c753a96e7 321 }
syundo0730 0:8d2c753a96e7 322
syundo0730 0:8d2c753a96e7 323 // reset interrupt flag and get INT_STATUS byte
syundo0730 0:8d2c753a96e7 324 mpuInterrupt = false;
syundo0730 0:8d2c753a96e7 325 mpuIntStatus = mpu.getIntStatus();
syundo0730 0:8d2c753a96e7 326
syundo0730 0:8d2c753a96e7 327 // get current FIFO count
syundo0730 0:8d2c753a96e7 328 fifoCount = mpu.getFIFOCount();
syundo0730 0:8d2c753a96e7 329
syundo0730 0:8d2c753a96e7 330 // check for overflow (this should never happen unless our code is too inefficient)
syundo0730 0:8d2c753a96e7 331 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
syundo0730 0:8d2c753a96e7 332 // reset so we can continue cleanly
syundo0730 0:8d2c753a96e7 333 mpu.resetFIFO();
syundo0730 0:8d2c753a96e7 334 //Serial.println(F("FIFO overflow!"));
syundo0730 0:8d2c753a96e7 335
syundo0730 0:8d2c753a96e7 336 // otherwise, check for DMP data ready interrupt (this should happen frequently)
syundo0730 0:8d2c753a96e7 337 } else if (mpuIntStatus & 0x02) {
syundo0730 0:8d2c753a96e7 338 // wait for correct available data length, should be a VERY short wait
syundo0730 0:8d2c753a96e7 339 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
syundo0730 0:8d2c753a96e7 340
syundo0730 0:8d2c753a96e7 341 // read a packet from FIFO
syundo0730 0:8d2c753a96e7 342 mpu.getFIFOBytes(fifoBuffer, packetSize);
syundo0730 0:8d2c753a96e7 343
syundo0730 0:8d2c753a96e7 344 // track FIFO count here in case there is > 1 packet available
syundo0730 0:8d2c753a96e7 345 // (this lets us immediately read more without waiting for an interrupt)
syundo0730 0:8d2c753a96e7 346 fifoCount -= packetSize;
syundo0730 0:8d2c753a96e7 347
majik 2:07e29b6d29da 348
majik 2:07e29b6d29da 349 printf("\n\r%.4f\t",t.read());
syundo0730 0:8d2c753a96e7 350 #ifdef OUTPUT_READABLE_QUATERNION
syundo0730 0:8d2c753a96e7 351 // display quaternion values in easy matrix form: w x y z
syundo0730 0:8d2c753a96e7 352 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 1:ec0a08108442 353 printf("quat\t");
syundo0730 1:ec0a08108442 354 printf("%f\t", q.w);
syundo0730 1:ec0a08108442 355 printf("%f\t", q.x);
syundo0730 1:ec0a08108442 356 printf("%f\t", q.y);
majik 2:07e29b6d29da 357 printf("%f\t", q.z);
syundo0730 0:8d2c753a96e7 358 #endif
syundo0730 0:8d2c753a96e7 359
syundo0730 0:8d2c753a96e7 360 #ifdef OUTPUT_READABLE_EULER
syundo0730 0:8d2c753a96e7 361 // display Euler angles in degrees
syundo0730 0:8d2c753a96e7 362 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 0:8d2c753a96e7 363 mpu.dmpGetEuler(euler, &q);
syundo0730 0:8d2c753a96e7 364 printf("euler\t");
syundo0730 0:8d2c753a96e7 365 printf("%f\t", euler[0] * 180/M_PI);
syundo0730 0:8d2c753a96e7 366 printf("%f\t", euler[1] * 180/M_PI);
majik 2:07e29b6d29da 367 printf("%f\t\", euler[2] * 180/M_PI);
syundo0730 0:8d2c753a96e7 368 #endif
syundo0730 0:8d2c753a96e7 369
syundo0730 0:8d2c753a96e7 370 #ifdef OUTPUT_READABLE_YAWPITCHROLL
syundo0730 0:8d2c753a96e7 371 // display Euler angles in degrees
syundo0730 0:8d2c753a96e7 372 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 0:8d2c753a96e7 373 mpu.dmpGetGravity(&gravity, &q);
syundo0730 0:8d2c753a96e7 374 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
syundo0730 0:8d2c753a96e7 375 printf("ypr\t");
majik 2:07e29b6d29da 376 printf("%.4f\t", ypr[0] * 180/M_PI);
majik 2:07e29b6d29da 377 printf("%.4f\t", ypr[1] * 180/M_PI);
majik 2:07e29b6d29da 378 printf("%.4f\t", ypr[2] * 180/M_PI);
syundo0730 0:8d2c753a96e7 379 #endif
syundo0730 0:8d2c753a96e7 380
syundo0730 0:8d2c753a96e7 381 #ifdef OUTPUT_READABLE_REALACCEL
syundo0730 0:8d2c753a96e7 382 // display real acceleration, adjusted to remove gravity
syundo0730 0:8d2c753a96e7 383 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 0:8d2c753a96e7 384 mpu.dmpGetAccel(&aa, fifoBuffer);
syundo0730 0:8d2c753a96e7 385 mpu.dmpGetGravity(&gravity, &q);
syundo0730 0:8d2c753a96e7 386 mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
syundo0730 1:ec0a08108442 387 printf("areal\t");
majik 2:07e29b6d29da 388 printf("%f.4\t", aaReal.x);
majik 2:07e29b6d29da 389 printf("%f.4\t", aaReal.y);
majik 2:07e29b6d29da 390 printf("%f.4\t", aaReal.z);
syundo0730 0:8d2c753a96e7 391 #endif
syundo0730 0:8d2c753a96e7 392
syundo0730 0:8d2c753a96e7 393 #ifdef OUTPUT_READABLE_WORLDACCEL
syundo0730 0:8d2c753a96e7 394 // display initial world-frame acceleration, adjusted to remove gravity
syundo0730 0:8d2c753a96e7 395 // and rotated based on known orientation from quaternion
syundo0730 0:8d2c753a96e7 396 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 0:8d2c753a96e7 397 mpu.dmpGetAccel(&aa, fifoBuffer);
syundo0730 0:8d2c753a96e7 398 mpu.dmpGetGravity(&gravity, &q);
syundo0730 0:8d2c753a96e7 399 mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
syundo0730 1:ec0a08108442 400 printf("aworld\t");
majik 2:07e29b6d29da 401 printf("%f.4\t", aaWorld.x);
majik 2:07e29b6d29da 402 printf("%f.4\t", aaWorld.y);
majik 2:07e29b6d29da 403 printf("%f.4\t", aaWorld.z);
syundo0730 0:8d2c753a96e7 404 #endif
syundo0730 0:8d2c753a96e7 405
syundo0730 0:8d2c753a96e7 406 #ifdef OUTPUT_TEAPOT
syundo0730 0:8d2c753a96e7 407 // display quaternion values in InvenSense Teapot demo format:
syundo0730 0:8d2c753a96e7 408 teapotPacket[2] = fifoBuffer[0];
syundo0730 0:8d2c753a96e7 409 teapotPacket[3] = fifoBuffer[1];
syundo0730 0:8d2c753a96e7 410 teapotPacket[4] = fifoBuffer[4];
syundo0730 0:8d2c753a96e7 411 teapotPacket[5] = fifoBuffer[5];
syundo0730 0:8d2c753a96e7 412 teapotPacket[6] = fifoBuffer[8];
syundo0730 0:8d2c753a96e7 413 teapotPacket[7] = fifoBuffer[9];
syundo0730 0:8d2c753a96e7 414 teapotPacket[8] = fifoBuffer[12];
syundo0730 0:8d2c753a96e7 415 teapotPacket[9] = fifoBuffer[13];
syundo0730 1:ec0a08108442 416 for (int i = 0; i < 14; ++i) {
syundo0730 1:ec0a08108442 417 pc.send(teapotPacket[i]);
syundo0730 1:ec0a08108442 418 }
syundo0730 0:8d2c753a96e7 419 teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
syundo0730 0:8d2c753a96e7 420 #endif
syundo0730 0:8d2c753a96e7 421
syundo0730 0:8d2c753a96e7 422 // blink LED to indicate activity
majik 2:07e29b6d29da 423 //led1 = !led1;
majik 2:07e29b6d29da 424 }
majik 2:07e29b6d29da 425 }
majik 2:07e29b6d29da 426
majik 2:07e29b6d29da 427
majik 2:07e29b6d29da 428 void loop2() {
majik 2:07e29b6d29da 429 // if programming failed, don't try to do anything
majik 2:07e29b6d29da 430 if (!dmpReady2) return;
majik 2:07e29b6d29da 431
majik 2:07e29b6d29da 432 // wait for MPU interrupt or extra packet(s) available
majik 2:07e29b6d29da 433 while (!mpuInterrupt2 && fifoCount2 < packetSize2) {
majik 2:07e29b6d29da 434 // other program behavior stuff here
majik 2:07e29b6d29da 435 // .
majik 2:07e29b6d29da 436 // .
majik 2:07e29b6d29da 437 // .
majik 2:07e29b6d29da 438 // if you are really paranoid you can frequently test in between other
majik 2:07e29b6d29da 439 // stuff to see if mpuInterrupt is true, and if so, "break;" from the
majik 2:07e29b6d29da 440 // while() loop to immediately process the MPU data
majik 2:07e29b6d29da 441 // .
majik 2:07e29b6d29da 442 // .
majik 2:07e29b6d29da 443 // .
majik 2:07e29b6d29da 444 //led1 = !led1;
majik 2:07e29b6d29da 445 //wait_ms(100);
majik 2:07e29b6d29da 446 }
majik 2:07e29b6d29da 447
majik 2:07e29b6d29da 448 // reset interrupt flag and get INT_STATUS byte
majik 2:07e29b6d29da 449 mpuInterrupt2 = false;
majik 2:07e29b6d29da 450 mpuIntStatus2 = mpu2.getIntStatus();
majik 2:07e29b6d29da 451
majik 2:07e29b6d29da 452 // get current FIFO count
majik 2:07e29b6d29da 453 fifoCount2 = mpu2.getFIFOCount();
majik 2:07e29b6d29da 454
majik 2:07e29b6d29da 455 // check for overflow (this should never happen unless our code is too inefficient)
majik 2:07e29b6d29da 456 if ((mpuIntStatus2 & 0x10) || fifoCount2 == 1024) {
majik 2:07e29b6d29da 457 // reset so we can continue cleanly
majik 2:07e29b6d29da 458 mpu2.resetFIFO();
majik 2:07e29b6d29da 459 //Serial.println(F("FIFO overflow!"));
majik 2:07e29b6d29da 460
majik 2:07e29b6d29da 461 // otherwise, check for DMP data ready interrupt (this should happen frequently)
majik 2:07e29b6d29da 462 } else if (mpuIntStatus2 & 0x02) {
majik 2:07e29b6d29da 463 // wait for correct available data length, should be a VERY short wait
majik 2:07e29b6d29da 464 while (fifoCount2 < packetSize2) fifoCount2 = mpu2.getFIFOCount();
majik 2:07e29b6d29da 465
majik 2:07e29b6d29da 466 // read a packet from FIFO
majik 2:07e29b6d29da 467 mpu2.getFIFOBytes(fifoBuffer2, packetSize2);
majik 2:07e29b6d29da 468
majik 2:07e29b6d29da 469 // track FIFO count here in case there is > 1 packet available
majik 2:07e29b6d29da 470 // (this lets us immediately read more without waiting for an interrupt)
majik 2:07e29b6d29da 471 fifoCount2 -= packetSize2;
majik 2:07e29b6d29da 472
majik 2:07e29b6d29da 473 #ifdef OUTPUT_READABLE_QUATERNION
majik 2:07e29b6d29da 474 // display quaternion values in easy matrix form: w x y z
majik 2:07e29b6d29da 475 mpu2.dmpGetQuaternion(&q, fifoBuffer2);
majik 2:07e29b6d29da 476 printf("quat\t");
majik 2:07e29b6d29da 477 printf("%.4f\t", q.w);
majik 2:07e29b6d29da 478 printf("%.4f\t", q.x);
majik 2:07e29b6d29da 479 printf("%.4f\t", q.y);
majik 2:07e29b6d29da 480 printf("%.4f\t", q.z);
majik 2:07e29b6d29da 481 #endif
majik 2:07e29b6d29da 482
majik 2:07e29b6d29da 483 #ifdef OUTPUT_READABLE_EULER
majik 2:07e29b6d29da 484 // display Euler angles in degrees
majik 2:07e29b6d29da 485 mpu2.dmpGetQuaternion(&q, fifoBuffer2);
majik 2:07e29b6d29da 486 mpu2.dmpGetEuler(euler, &q);
majik 2:07e29b6d29da 487 printf("euler\t");
majik 2:07e29b6d29da 488 printf("%.4f\t", euler2[0] * 180/M_PI);
majik 2:07e29b6d29da 489 printf("%.4f\t", euler2[1] * 180/M_PI);
majik 2:07e29b6d29da 490 printf("%.4f\t", euler2[2] * 180/M_PI);
majik 2:07e29b6d29da 491 #endif
majik 2:07e29b6d29da 492
majik 2:07e29b6d29da 493 #ifdef OUTPUT_READABLE_YAWPITCHROLL
majik 2:07e29b6d29da 494 // display Euler angles in degrees
majik 2:07e29b6d29da 495 mpu2.dmpGetQuaternion(&q, fifoBuffer2);
majik 2:07e29b6d29da 496 mpu2.dmpGetGravity(&gravity, &q);
majik 2:07e29b6d29da 497 mpu2.dmpGetYawPitchRoll(ypr, &q, &gravity);
majik 2:07e29b6d29da 498 printf("ypr\t");
majik 2:07e29b6d29da 499 printf("%.4f\t", ypr[0] * 180/M_PI);
majik 2:07e29b6d29da 500 printf("%.4f\t", ypr[1] * 180/M_PI);
majik 2:07e29b6d29da 501 printf("%.4f\t", ypr[2] * 180/M_PI);
majik 2:07e29b6d29da 502 #endif
majik 2:07e29b6d29da 503
majik 2:07e29b6d29da 504 #ifdef OUTPUT_READABLE_REALACCEL
majik 2:07e29b6d29da 505 // display real acceleration, adjusted to remove gravity
majik 2:07e29b6d29da 506 mpu.dmpGetQuaternion(&q, fifoBuffer);
majik 2:07e29b6d29da 507 mpu.dmpGetAccel(&aa, fifoBuffer);
majik 2:07e29b6d29da 508 mpu.dmpGetGravity(&gravity, &q);
majik 2:07e29b6d29da 509 mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
majik 2:07e29b6d29da 510 printf("areal\t");
majik 2:07e29b6d29da 511 printf("%.4f\t", aaReal.x);
majik 2:07e29b6d29da 512 printf("%.4f\t", aaReal.y);
majik 2:07e29b6d29da 513 printf("%.4f\t\r\n", aaReal.z);
majik 2:07e29b6d29da 514 #endif
majik 2:07e29b6d29da 515
majik 2:07e29b6d29da 516 #ifdef OUTPUT_READABLE_WORLDACCEL
majik 2:07e29b6d29da 517 // display initial world-frame acceleration, adjusted to remove gravity
majik 2:07e29b6d29da 518 // and rotated based on known orientation from quaternion
majik 2:07e29b6d29da 519 mpu.dmpGetQuaternion(&q, fifoBuffer);
majik 2:07e29b6d29da 520 mpu.dmpGetAccel(&aa, fifoBuffer);
majik 2:07e29b6d29da 521 mpu.dmpGetGravity(&gravity, &q);
majik 2:07e29b6d29da 522 mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
majik 2:07e29b6d29da 523 printf("aworld\t");
majik 2:07e29b6d29da 524 printf("%f\t", aaWorld.x);
majik 2:07e29b6d29da 525 printf("%f\t", aaWorld.y);
majik 2:07e29b6d29da 526 printf("%f\t\r\n", aaWorld.z);
majik 2:07e29b6d29da 527 #endif
majik 2:07e29b6d29da 528 /*
majik 2:07e29b6d29da 529 #ifdef OUTPUT_TEAPOT
majik 2:07e29b6d29da 530 // display quaternion values in InvenSense Teapot demo format:
majik 2:07e29b6d29da 531 teapotPacket[2] = fifoBuffer[0];
majik 2:07e29b6d29da 532 teapotPacket[3] = fifoBuffer[1];
majik 2:07e29b6d29da 533 teapotPacket[4] = fifoBuffer[4];
majik 2:07e29b6d29da 534 teapotPacket[5] = fifoBuffer[5];
majik 2:07e29b6d29da 535 teapotPacket[6] = fifoBuffer[8];
majik 2:07e29b6d29da 536 teapotPacket[7] = fifoBuffer[9];
majik 2:07e29b6d29da 537 teapotPacket[8] = fifoBuffer[12];
majik 2:07e29b6d29da 538 teapotPacket[9] = fifoBuffer[13];
majik 2:07e29b6d29da 539 for (int i = 0; i < 14; ++i) {
majik 2:07e29b6d29da 540 pc.send(teapotPacket[i]);
majik 2:07e29b6d29da 541 }
majik 2:07e29b6d29da 542 teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
majik 2:07e29b6d29da 543 #endif*/
majik 2:07e29b6d29da 544 printf("\r\n");
majik 2:07e29b6d29da 545 // blink LED to indicate activity
majik 2:07e29b6d29da 546 //led1 = !led1;
syundo0730 0:8d2c753a96e7 547 }
syundo0730 0:8d2c753a96e7 548 }