mohamed askar
/
MPU6050-DMP_6DOF
mpu6050 dmp test
Fork of MPU6050-DMP_6DOF by
main.cpp@8:f8dc6caa1e2a, 2015-04-05 (annotated)
- Committer:
- MohamedAskar
- Date:
- Sun Apr 05 18:40:55 2015 +0000
- Revision:
- 8:f8dc6caa1e2a
- Parent:
- 7:9797eb1bdc10
mpu6050 dmp test
Who changed what in which revision?
User | Revision | Line number | New 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) |
MohamedAskar | 8:f8dc6caa1e2a | 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. |
MohamedAskar | 8:f8dc6caa1e2a | 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 |
khaledelmadawi | 7:9797eb1bdc10 | 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); |
khaledelmadawi | 7:9797eb1bdc10 | 330 | pc.printf("ypr\t\r\n"); |
khaledelmadawi | 7:9797eb1bdc10 | 331 | pc.printf("%f3.2\r\n",ypr[0] * 180/M_PI); |
rolfz | 6:029b4b8111ef | 332 | pc.printf("\t"); |
khaledelmadawi | 7:9797eb1bdc10 | 333 | pc.printf("%f3.2\r\n",ypr[1] * 180/M_PI); |
rolfz | 6:029b4b8111ef | 334 | pc.printf("\t"); |
khaledelmadawi | 7:9797eb1bdc10 | 335 | pc.printf("%f3.2\r\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 | } |