![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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
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?
User | Revision | Line number | New 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 | } |