1
Dependencies: ArduinoSerial I2Cdev
Fork of MPU6050 by
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?
User | Revision | Line number | New 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 | } |