aaa
Dependencies: ArduinoSerial I2Cdev2
Dependents: AutoFlight2017_now2 AutoFlight2018_Control sbus_test_2018 Autoflight2018_sbusread ... more
MPU6050_DMP6.cpp@0:c3af3416e383, 2018-08-28 (annotated)
- Committer:
- TUATBM
- Date:
- Tue Aug 28 07:09:21 2018 +0000
- Revision:
- 0:c3af3416e383
- Child:
- 2:ff025b9c6c7c
a
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
TUATBM | 0:c3af3416e383 | 1 | #include "MPU6050_DMP6.h" |
TUATBM | 0:c3af3416e383 | 2 | |
TUATBM | 0:c3af3416e383 | 3 | // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files |
TUATBM | 0:c3af3416e383 | 4 | // for both classes must be in the include path of your project |
TUATBM | 0:c3af3416e383 | 5 | |
TUATBM | 0:c3af3416e383 | 6 | //#include "MPU6050.h" // not necessary if using MotionApps include file |
TUATBM | 0:c3af3416e383 | 7 | |
TUATBM | 0:c3af3416e383 | 8 | |
TUATBM | 0:c3af3416e383 | 9 | |
TUATBM | 0:c3af3416e383 | 10 | /* ========================================================================= |
TUATBM | 0:c3af3416e383 | 11 | NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch |
TUATBM | 0:c3af3416e383 | 12 | depends on the MPU-6050's INT pin being connected to the Arduino's |
TUATBM | 0:c3af3416e383 | 13 | external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is |
TUATBM | 0:c3af3416e383 | 14 | digital I/O pin 2. |
TUATBM | 0:c3af3416e383 | 15 | * ========================================================================= */ |
TUATBM | 0:c3af3416e383 | 16 | |
TUATBM | 0:c3af3416e383 | 17 | // uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual |
TUATBM | 0:c3af3416e383 | 18 | // quaternion components in a [w, x, y, z] format (not best for parsing |
TUATBM | 0:c3af3416e383 | 19 | // on a remote host such as Processing or something though) |
TUATBM | 0:c3af3416e383 | 20 | //#define OUTPUT_READABLE_QUATERNION |
TUATBM | 0:c3af3416e383 | 21 | |
TUATBM | 0:c3af3416e383 | 22 | // uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles |
TUATBM | 0:c3af3416e383 | 23 | // (in degrees) calculated from the quaternions coming from the FIFO. |
TUATBM | 0:c3af3416e383 | 24 | // Note that Euler angles suffer from gimbal lock (for more info, see |
TUATBM | 0:c3af3416e383 | 25 | // http://en.wikipedia.org/wiki/Gimbal_lock) |
TUATBM | 0:c3af3416e383 | 26 | //#define OUTPUT_READABLE_EULER |
TUATBM | 0:c3af3416e383 | 27 | |
TUATBM | 0:c3af3416e383 | 28 | // uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/ |
TUATBM | 0:c3af3416e383 | 29 | // pitch/roll angles (in degrees) calculated from the quaternions coming |
TUATBM | 0:c3af3416e383 | 30 | // from the FIFO. Note this also requires gravity vector calculations. |
TUATBM | 0:c3af3416e383 | 31 | // Also note that yaw/pitch/roll angles suffer from gimbal lock (for |
TUATBM | 0:c3af3416e383 | 32 | // more info, see: http://en.wikipedia.org/wiki/Gimbal_lock) |
TUATBM | 0:c3af3416e383 | 33 | #define OUTPUT_READABLE_YAWPITCHROLL |
TUATBM | 0:c3af3416e383 | 34 | |
TUATBM | 0:c3af3416e383 | 35 | // uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration |
TUATBM | 0:c3af3416e383 | 36 | // components with gravity removed. This acceleration reference frame is |
TUATBM | 0:c3af3416e383 | 37 | // not compensated for orientation, so +X is always +X according to the |
TUATBM | 0:c3af3416e383 | 38 | // sensor, just without the effects of gravity. If you want acceleration |
TUATBM | 0:c3af3416e383 | 39 | // compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead. |
TUATBM | 0:c3af3416e383 | 40 | //#define OUTPUT_READABLE_REALACCEL |
TUATBM | 0:c3af3416e383 | 41 | |
TUATBM | 0:c3af3416e383 | 42 | // uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration |
TUATBM | 0:c3af3416e383 | 43 | // components with gravity removed and adjusted for the world frame of |
TUATBM | 0:c3af3416e383 | 44 | // reference (yaw is relative to initial orientation, since no magnetometer |
TUATBM | 0:c3af3416e383 | 45 | // is present in this case). Could be quite handy in some cases. |
TUATBM | 0:c3af3416e383 | 46 | //#define OUTPUT_READABLE_WORLDACCEL |
TUATBM | 0:c3af3416e383 | 47 | |
TUATBM | 0:c3af3416e383 | 48 | // uncomment "OUTPUT_TEAPOT" if you want output that matches the |
TUATBM | 0:c3af3416e383 | 49 | // format used for the InvenSense teapot demo |
TUATBM | 0:c3af3416e383 | 50 | //#define OUTPUT_TEAPOT |
TUATBM | 0:c3af3416e383 | 51 | |
TUATBM | 0:c3af3416e383 | 52 | #ifndef M_PI |
TUATBM | 0:c3af3416e383 | 53 | #define M_PI 3.14159265358979 |
TUATBM | 0:c3af3416e383 | 54 | #endif |
TUATBM | 0:c3af3416e383 | 55 | |
TUATBM | 0:c3af3416e383 | 56 | MPU6050DMP6::MPU6050DMP6(PinName intpin, RawSerial* serial_p) |
TUATBM | 0:c3af3416e383 | 57 | : |
TUATBM | 0:c3af3416e383 | 58 | checkpin_p(new InterruptIn(intpin)), |
TUATBM | 0:c3af3416e383 | 59 | checkpin(*checkpin_p), |
TUATBM | 0:c3af3416e383 | 60 | pc_p(serial_p) |
TUATBM | 0:c3af3416e383 | 61 | { |
TUATBM | 0:c3af3416e383 | 62 | initializeValue(); |
TUATBM | 0:c3af3416e383 | 63 | } |
TUATBM | 0:c3af3416e383 | 64 | |
TUATBM | 0:c3af3416e383 | 65 | MPU6050DMP6::~MPU6050DMP6(){} |
TUATBM | 0:c3af3416e383 | 66 | |
TUATBM | 0:c3af3416e383 | 67 | void MPU6050DMP6::dmpDataReady() |
TUATBM | 0:c3af3416e383 | 68 | { |
TUATBM | 0:c3af3416e383 | 69 | mpuInterrupt = true; |
TUATBM | 0:c3af3416e383 | 70 | } |
TUATBM | 0:c3af3416e383 | 71 | |
TUATBM | 0:c3af3416e383 | 72 | void MPU6050DMP6::transformeCoordinate_Skipper(Quaternion* q1, Quaternion* q2){ |
TUATBM | 0:c3af3416e383 | 73 | q2->w = q1->w; |
TUATBM | 0:c3af3416e383 | 74 | q2->x = q1->y; |
TUATBM | 0:c3af3416e383 | 75 | q2->y = - q1->x; |
TUATBM | 0:c3af3416e383 | 76 | q2->z = q1->z; |
TUATBM | 0:c3af3416e383 | 77 | } |
TUATBM | 0:c3af3416e383 | 78 | |
TUATBM | 0:c3af3416e383 | 79 | |
TUATBM | 0:c3af3416e383 | 80 | int MPU6050DMP6::loopstarting(){ |
TUATBM | 0:c3af3416e383 | 81 | // if programming failed, don't try to do anything |
TUATBM | 0:c3af3416e383 | 82 | if (!dmpReady) return -1; |
TUATBM | 0:c3af3416e383 | 83 | |
TUATBM | 0:c3af3416e383 | 84 | // wait for MPU interrupt or extra packet(s) available |
TUATBM | 0:c3af3416e383 | 85 | while (!mpuInterrupt && fifoCount < packetSize) { |
TUATBM | 0:c3af3416e383 | 86 | // other program behavior stuff here |
TUATBM | 0:c3af3416e383 | 87 | // . |
TUATBM | 0:c3af3416e383 | 88 | // . |
TUATBM | 0:c3af3416e383 | 89 | // . |
TUATBM | 0:c3af3416e383 | 90 | // if you are really paranoid you can frequently test in between other |
TUATBM | 0:c3af3416e383 | 91 | // stuff to see if mpuInterrupt is true, and if so, "break;" from the |
TUATBM | 0:c3af3416e383 | 92 | // while() loop to immediately process the MPU data |
TUATBM | 0:c3af3416e383 | 93 | // . |
TUATBM | 0:c3af3416e383 | 94 | // . |
TUATBM | 0:c3af3416e383 | 95 | // . |
TUATBM | 0:c3af3416e383 | 96 | } |
TUATBM | 0:c3af3416e383 | 97 | |
TUATBM | 0:c3af3416e383 | 98 | // reset interrupt flag and get INT_STATUS byte |
TUATBM | 0:c3af3416e383 | 99 | mpuInterrupt = false; |
TUATBM | 0:c3af3416e383 | 100 | mpuIntStatus = mpu.getIntStatus(); |
TUATBM | 0:c3af3416e383 | 101 | |
TUATBM | 0:c3af3416e383 | 102 | // get current FIFO count |
TUATBM | 0:c3af3416e383 | 103 | fifoCount = mpu.getFIFOCount(); |
TUATBM | 0:c3af3416e383 | 104 | |
TUATBM | 0:c3af3416e383 | 105 | // check for overflow (this should never happen unless our code is too inefficient) |
TUATBM | 0:c3af3416e383 | 106 | if ((mpuIntStatus & 0x10) || fifoCount == 1024) { |
TUATBM | 0:c3af3416e383 | 107 | // reset so we can continue cleanly |
TUATBM | 0:c3af3416e383 | 108 | mpu.resetFIFO(); |
TUATBM | 0:c3af3416e383 | 109 | //arduinoSerial.println(F("FIFO overflow!")); |
TUATBM | 0:c3af3416e383 | 110 | |
TUATBM | 0:c3af3416e383 | 111 | // otherwise, check for DMP data ready interrupt (this should happen frequently) |
TUATBM | 0:c3af3416e383 | 112 | } else if (mpuIntStatus & 0x02) { |
TUATBM | 0:c3af3416e383 | 113 | // wait for correct available data length, should be a VERY short wait |
TUATBM | 0:c3af3416e383 | 114 | while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); |
TUATBM | 0:c3af3416e383 | 115 | |
TUATBM | 0:c3af3416e383 | 116 | // read a packet from FIFO |
TUATBM | 0:c3af3416e383 | 117 | mpu.getFIFOBytes(fifoBuffer, packetSize); |
TUATBM | 0:c3af3416e383 | 118 | |
TUATBM | 0:c3af3416e383 | 119 | // track FIFO count here in case there is > 1 packet available |
TUATBM | 0:c3af3416e383 | 120 | // (this lets us immediately read more without waiting for an interrupt) |
TUATBM | 0:c3af3416e383 | 121 | fifoCount -= packetSize; |
TUATBM | 0:c3af3416e383 | 122 | |
TUATBM | 0:c3af3416e383 | 123 | } |
TUATBM | 0:c3af3416e383 | 124 | |
TUATBM | 0:c3af3416e383 | 125 | |
TUATBM | 0:c3af3416e383 | 126 | } |
TUATBM | 0:c3af3416e383 | 127 | // ================================================================ |
TUATBM | 0:c3af3416e383 | 128 | // === INITIAL SETUP === |
TUATBM | 0:c3af3416e383 | 129 | // ================================================================ |
TUATBM | 0:c3af3416e383 | 130 | |
TUATBM | 0:c3af3416e383 | 131 | int MPU6050DMP6::setup() |
TUATBM | 0:c3af3416e383 | 132 | { |
TUATBM | 0:c3af3416e383 | 133 | // initialize arduinoSerial communication |
TUATBM | 0:c3af3416e383 | 134 | // (115200 chosen because it is required for Teapot Demo output, but it's |
TUATBM | 0:c3af3416e383 | 135 | // really up to you depending on your project) |
TUATBM | 0:c3af3416e383 | 136 | // initialize device |
TUATBM | 0:c3af3416e383 | 137 | pc_p->printf("Initializing I2C devices...\r\n"); |
TUATBM | 0:c3af3416e383 | 138 | mpu.initialize(); |
TUATBM | 0:c3af3416e383 | 139 | |
TUATBM | 0:c3af3416e383 | 140 | // verify connection |
TUATBM | 0:c3af3416e383 | 141 | pc_p->printf("Testing device connections...\r\n"); |
TUATBM | 0:c3af3416e383 | 142 | |
TUATBM | 0:c3af3416e383 | 143 | if(mpu.testConnection()) pc_p->printf("MPU6050 connection successful\r\n"); |
TUATBM | 0:c3af3416e383 | 144 | else pc_p->printf("MPU6050 connection failed\r\n"); |
TUATBM | 0:c3af3416e383 | 145 | |
TUATBM | 0:c3af3416e383 | 146 | //arduinoSerial.println(F("Testing device connections...")); |
TUATBM | 0:c3af3416e383 | 147 | //arduinoSerial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); |
TUATBM | 0:c3af3416e383 | 148 | |
TUATBM | 0:c3af3416e383 | 149 | // wait for ready |
TUATBM | 0:c3af3416e383 | 150 | // arduinoSerial.println(F("\nSend any character to begin DMP programming and demo: ")); |
TUATBM | 0:c3af3416e383 | 151 | // while (arduinoSerial.available() && arduinoSerial.read()); // empty buffer |
TUATBM | 0:c3af3416e383 | 152 | // while (!arduinoSerial.available()); // wait for data |
TUATBM | 0:c3af3416e383 | 153 | // while (arduinoSerial.available() && arduinoSerial.read()); // empty buffer again |
TUATBM | 0:c3af3416e383 | 154 | |
TUATBM | 0:c3af3416e383 | 155 | // load and configure the DMP |
TUATBM | 0:c3af3416e383 | 156 | pc_p->printf("Initializing DMP...\r\n"); |
TUATBM | 0:c3af3416e383 | 157 | //arduinoSerial.println(F("Initializing DMP...")); |
TUATBM | 0:c3af3416e383 | 158 | devStatus = mpu.dmpInitialize(); |
TUATBM | 0:c3af3416e383 | 159 | |
TUATBM | 0:c3af3416e383 | 160 | // supply your own gyro offsets here, scaled for min sensitivity |
TUATBM | 0:c3af3416e383 | 161 | mpu.setXGyroOffset(220); |
TUATBM | 0:c3af3416e383 | 162 | mpu.setYGyroOffset(76); |
TUATBM | 0:c3af3416e383 | 163 | mpu.setZGyroOffset(-85); |
TUATBM | 0:c3af3416e383 | 164 | mpu.setZAccelOffset(1788); // 1688 factory default for my test chip |
TUATBM | 0:c3af3416e383 | 165 | |
TUATBM | 0:c3af3416e383 | 166 | // make sure it worked (returns 0 if so) |
TUATBM | 0:c3af3416e383 | 167 | if (devStatus == 0) { |
TUATBM | 0:c3af3416e383 | 168 | // turn on the DMP, now that it's ready |
TUATBM | 0:c3af3416e383 | 169 | pc_p->printf("Enabling DMP...\r\n"); |
TUATBM | 0:c3af3416e383 | 170 | |
TUATBM | 0:c3af3416e383 | 171 | //arduinoSerial.println(F("Enabling DMP...")); |
TUATBM | 0:c3af3416e383 | 172 | mpu.setDMPEnabled(true); |
TUATBM | 0:c3af3416e383 | 173 | |
TUATBM | 0:c3af3416e383 | 174 | // enable Arduino interrupt detection |
TUATBM | 0:c3af3416e383 | 175 | pc_p->printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n"); |
TUATBM | 0:c3af3416e383 | 176 | //arduinoSerial.println(F("Enabling interrupt detection (Arduino external interrupt 0)...")); |
TUATBM | 0:c3af3416e383 | 177 | checkpin.rise(this, &MPU6050DMP6::dmpDataReady); |
TUATBM | 0:c3af3416e383 | 178 | mpuIntStatus = mpu.getIntStatus(); |
TUATBM | 0:c3af3416e383 | 179 | |
TUATBM | 0:c3af3416e383 | 180 | // set our DMP Ready flag so the main loop() function knows it's okay to use it |
TUATBM | 0:c3af3416e383 | 181 | pc_p->printf("DMP ready! Waiting for first interrupt...\r\n"); |
TUATBM | 0:c3af3416e383 | 182 | //arduinoSerial.println(F("DMP ready! Waiting for first interrupt...")); |
TUATBM | 0:c3af3416e383 | 183 | dmpReady = true; |
TUATBM | 0:c3af3416e383 | 184 | |
TUATBM | 0:c3af3416e383 | 185 | // get expected DMP packet size for later comparison |
TUATBM | 0:c3af3416e383 | 186 | packetSize = mpu.dmpGetFIFOPacketSize(); |
TUATBM | 0:c3af3416e383 | 187 | } else { |
TUATBM | 0:c3af3416e383 | 188 | // ERROR! |
TUATBM | 0:c3af3416e383 | 189 | // 1 = initial memory load failed |
TUATBM | 0:c3af3416e383 | 190 | // 2 = DMP configuration updates failed |
TUATBM | 0:c3af3416e383 | 191 | // (if it's going to break, usually the code will be 1) |
TUATBM | 0:c3af3416e383 | 192 | |
TUATBM | 0:c3af3416e383 | 193 | pc_p->printf("DMP Initialization failed (code %d)\r\n",devStatus); |
TUATBM | 0:c3af3416e383 | 194 | |
TUATBM | 0:c3af3416e383 | 195 | |
TUATBM | 0:c3af3416e383 | 196 | //arduinoSerial.print(F("DMP Initialization failed (code ")); |
TUATBM | 0:c3af3416e383 | 197 | //arduinoSerial.print(devStatus); |
TUATBM | 0:c3af3416e383 | 198 | //arduinoSerial.println(F(")")); |
TUATBM | 0:c3af3416e383 | 199 | return -1; |
TUATBM | 0:c3af3416e383 | 200 | } |
TUATBM | 0:c3af3416e383 | 201 | |
TUATBM | 0:c3af3416e383 | 202 | return 0; |
TUATBM | 0:c3af3416e383 | 203 | } |
TUATBM | 0:c3af3416e383 | 204 | |
TUATBM | 0:c3af3416e383 | 205 | |
TUATBM | 0:c3af3416e383 | 206 | |
TUATBM | 0:c3af3416e383 | 207 | // ================================================================ |
TUATBM | 0:c3af3416e383 | 208 | // === MAIN PROGRAM LOOP === |
TUATBM | 0:c3af3416e383 | 209 | // ================================================================ |
TUATBM | 0:c3af3416e383 | 210 | |
TUATBM | 0:c3af3416e383 | 211 | void MPU6050DMP6::loop() |
TUATBM | 0:c3af3416e383 | 212 | { |
TUATBM | 0:c3af3416e383 | 213 | loopstarting(); |
TUATBM | 0:c3af3416e383 | 214 | |
TUATBM | 0:c3af3416e383 | 215 | #ifdef OUTPUT_READABLE_QUATERNION |
TUATBM | 0:c3af3416e383 | 216 | // display quaternion values in easy matrix form: w x y z |
TUATBM | 0:c3af3416e383 | 217 | mpu.dmpGetQuaternion(&q, fifoBuffer); |
TUATBM | 0:c3af3416e383 | 218 | /* |
TUATBM | 0:c3af3416e383 | 219 | arduinoSerial.print("quat\t"); |
TUATBM | 0:c3af3416e383 | 220 | arduinoSerial.print(q.w); |
TUATBM | 0:c3af3416e383 | 221 | arduinoSerial.print("\t"); |
TUATBM | 0:c3af3416e383 | 222 | arduinoSerial.print(q.x); |
TUATBM | 0:c3af3416e383 | 223 | arduinoSerial.print("\t"); |
TUATBM | 0:c3af3416e383 | 224 | arduinoSerial.print(q.y); |
TUATBM | 0:c3af3416e383 | 225 | arduinoSerial.print("\t"); |
TUATBM | 0:c3af3416e383 | 226 | arduinoSerial.println(q.z); |
TUATBM | 0:c3af3416e383 | 227 | */ |
TUATBM | 0:c3af3416e383 | 228 | #endif |
TUATBM | 0:c3af3416e383 | 229 | |
TUATBM | 0:c3af3416e383 | 230 | #ifdef OUTPUT_READABLE_EULER |
TUATBM | 0:c3af3416e383 | 231 | // display Euler angles in degrees |
TUATBM | 0:c3af3416e383 | 232 | mpu.dmpGetQuaternion(&q, fifoBuffer); |
TUATBM | 0:c3af3416e383 | 233 | mpu.dmpGetEuler(euler, &q); |
TUATBM | 0:c3af3416e383 | 234 | /* |
TUATBM | 0:c3af3416e383 | 235 | arduinoSerial.print("euler\t"); |
TUATBM | 0:c3af3416e383 | 236 | arduinoSerial.print(euler[0] * 180/M_PI); |
TUATBM | 0:c3af3416e383 | 237 | arduinoSerial.print("\t"); |
TUATBM | 0:c3af3416e383 | 238 | arduinoSerial.print(euler[1] * 180/M_PI); |
TUATBM | 0:c3af3416e383 | 239 | arduinoSerial.print("\t"); |
TUATBM | 0:c3af3416e383 | 240 | arduinoSerial.println(euler[2] * 180/M_PI); |
TUATBM | 0:c3af3416e383 | 241 | */ |
TUATBM | 0:c3af3416e383 | 242 | #endif |
TUATBM | 0:c3af3416e383 | 243 | |
TUATBM | 0:c3af3416e383 | 244 | #ifdef OUTPUT_READABLE_YAWPITCHROLL |
TUATBM | 0:c3af3416e383 | 245 | // display Euler angles in degrees |
TUATBM | 0:c3af3416e383 | 246 | mpu.dmpGetQuaternion(&q, fifoBuffer); |
TUATBM | 0:c3af3416e383 | 247 | mpu.dmpGetGravity(&gravity, &q); |
TUATBM | 0:c3af3416e383 | 248 | mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); |
TUATBM | 0:c3af3416e383 | 249 | /* |
TUATBM | 0:c3af3416e383 | 250 | arduinoSerial.print("ypr\t"); |
TUATBM | 0:c3af3416e383 | 251 | arduinoSerial.print(ypr[0] * 180/M_PI); |
TUATBM | 0:c3af3416e383 | 252 | arduinoSerial.print("\t"); |
TUATBM | 0:c3af3416e383 | 253 | arduinoSerial.print(ypr[1] * 180/M_PI); |
TUATBM | 0:c3af3416e383 | 254 | arduinoSerial.print("\t"); |
TUATBM | 0:c3af3416e383 | 255 | arduinoSerial.println(ypr[2] * 180/M_PI); |
TUATBM | 0:c3af3416e383 | 256 | */ |
TUATBM | 0:c3af3416e383 | 257 | #endif |
TUATBM | 0:c3af3416e383 | 258 | |
TUATBM | 0:c3af3416e383 | 259 | #ifdef OUTPUT_READABLE_REALACCEL |
TUATBM | 0:c3af3416e383 | 260 | // display real acceleration, adjusted to remove gravity |
TUATBM | 0:c3af3416e383 | 261 | mpu.dmpGetQuaternion(&q, fifoBuffer); |
TUATBM | 0:c3af3416e383 | 262 | mpu.dmpGetAccel(&aa, fifoBuffer); |
TUATBM | 0:c3af3416e383 | 263 | mpu.dmpGetGravity(&gravity, &q); |
TUATBM | 0:c3af3416e383 | 264 | mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); |
TUATBM | 0:c3af3416e383 | 265 | /* |
TUATBM | 0:c3af3416e383 | 266 | arduinoSerial.print("areal\t"); |
TUATBM | 0:c3af3416e383 | 267 | arduinoSerial.print(aaReal.x); |
TUATBM | 0:c3af3416e383 | 268 | arduinoSerial.print("\t"); |
TUATBM | 0:c3af3416e383 | 269 | arduinoSerial.print(aaReal.y); |
TUATBM | 0:c3af3416e383 | 270 | arduinoSerial.print("\t"); |
TUATBM | 0:c3af3416e383 | 271 | arduinoSerial.println(aaReal.z); |
TUATBM | 0:c3af3416e383 | 272 | */ |
TUATBM | 0:c3af3416e383 | 273 | #endif |
TUATBM | 0:c3af3416e383 | 274 | |
TUATBM | 0:c3af3416e383 | 275 | #ifdef OUTPUT_READABLE_WORLDACCEL |
TUATBM | 0:c3af3416e383 | 276 | // display initial world-frame acceleration, adjusted to remove gravity |
TUATBM | 0:c3af3416e383 | 277 | // and rotated based on known orientation from quaternion |
TUATBM | 0:c3af3416e383 | 278 | mpu.dmpGetQuaternion(&q, fifoBuffer); |
TUATBM | 0:c3af3416e383 | 279 | mpu.dmpGetAccel(&aa, fifoBuffer); |
TUATBM | 0:c3af3416e383 | 280 | mpu.dmpGetGravity(&gravity, &q); |
TUATBM | 0:c3af3416e383 | 281 | mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); |
TUATBM | 0:c3af3416e383 | 282 | mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); |
TUATBM | 0:c3af3416e383 | 283 | /* |
TUATBM | 0:c3af3416e383 | 284 | arduinoSerial.print("aworld\t"); |
TUATBM | 0:c3af3416e383 | 285 | arduinoSerial.print(aaWorld.x); |
TUATBM | 0:c3af3416e383 | 286 | arduinoSerial.print("\t"); |
TUATBM | 0:c3af3416e383 | 287 | arduinoSerial.print(aaWorld.y); |
TUATBM | 0:c3af3416e383 | 288 | arduinoSerial.print("\t"); |
TUATBM | 0:c3af3416e383 | 289 | arduinoSerial.println(aaWorld.z); |
TUATBM | 0:c3af3416e383 | 290 | */ |
TUATBM | 0:c3af3416e383 | 291 | #endif |
TUATBM | 0:c3af3416e383 | 292 | |
TUATBM | 0:c3af3416e383 | 293 | #ifdef OUTPUT_TEAPOT |
TUATBM | 0:c3af3416e383 | 294 | // display quaternion values in InvenSense Teapot demo format: |
TUATBM | 0:c3af3416e383 | 295 | teapotPacket[2] = fifoBuffer[0]; |
TUATBM | 0:c3af3416e383 | 296 | teapotPacket[3] = fifoBuffer[1]; |
TUATBM | 0:c3af3416e383 | 297 | teapotPacket[4] = fifoBuffer[4]; |
TUATBM | 0:c3af3416e383 | 298 | teapotPacket[5] = fifoBuffer[5]; |
TUATBM | 0:c3af3416e383 | 299 | teapotPacket[6] = fifoBuffer[8]; |
TUATBM | 0:c3af3416e383 | 300 | teapotPacket[7] = fifoBuffer[9]; |
TUATBM | 0:c3af3416e383 | 301 | teapotPacket[8] = fifoBuffer[12]; |
TUATBM | 0:c3af3416e383 | 302 | teapotPacket[9] = fifoBuffer[13]; |
TUATBM | 0:c3af3416e383 | 303 | //arduinoSerial.write(teapotPacket, 14); |
TUATBM | 0:c3af3416e383 | 304 | teapotPacket[11]++; // packetCount, loops at 0xFF on purpose |
TUATBM | 0:c3af3416e383 | 305 | #endif |
TUATBM | 0:c3af3416e383 | 306 | |
TUATBM | 0:c3af3416e383 | 307 | } |
TUATBM | 0:c3af3416e383 | 308 | |
TUATBM | 0:c3af3416e383 | 309 | |
TUATBM | 0:c3af3416e383 | 310 | void MPU6050DMP6::getRollPitchYaw_Skipper(float rpy[3]){ |
TUATBM | 0:c3af3416e383 | 311 | float ypr[3]; |
TUATBM | 0:c3af3416e383 | 312 | loopstarting(); |
TUATBM | 0:c3af3416e383 | 313 | // display Euler angles in degrees |
TUATBM | 0:c3af3416e383 | 314 | mpu.dmpGetQuaternion(&q, fifoBuffer); |
TUATBM | 0:c3af3416e383 | 315 | transformeCoordinate_Skipper(&q, &q2); |
TUATBM | 0:c3af3416e383 | 316 | mpu.dmpGetGravity(&gravity, &q2); |
TUATBM | 0:c3af3416e383 | 317 | mpu.dmpGetYawPitchRoll(ypr, &q2, &gravity); |
TUATBM | 0:c3af3416e383 | 318 | rpy[0] = ypr[2]; |
TUATBM | 0:c3af3416e383 | 319 | rpy[1] = ypr[1]; |
TUATBM | 0:c3af3416e383 | 320 | rpy[2] = ypr[0]; |
TUATBM | 0:c3af3416e383 | 321 | } |
TUATBM | 0:c3af3416e383 | 322 | |
TUATBM | 0:c3af3416e383 | 323 | void MPU6050DMP6::initializeValue(){ |
TUATBM | 0:c3af3416e383 | 324 | dmpReady = false; |
TUATBM | 0:c3af3416e383 | 325 | mpuInterrupt = false; |
TUATBM | 0:c3af3416e383 | 326 | |
TUATBM | 0:c3af3416e383 | 327 | teapotPacket[0] = '$'; |
TUATBM | 0:c3af3416e383 | 328 | teapotPacket[1] = 0x20; |
TUATBM | 0:c3af3416e383 | 329 | teapotPacket[2] = 0; |
TUATBM | 0:c3af3416e383 | 330 | teapotPacket[3] = 0; |
TUATBM | 0:c3af3416e383 | 331 | teapotPacket[4] = 0; |
TUATBM | 0:c3af3416e383 | 332 | teapotPacket[5] = 0; |
TUATBM | 0:c3af3416e383 | 333 | teapotPacket[6] = 0; |
TUATBM | 0:c3af3416e383 | 334 | teapotPacket[7] = 0; |
TUATBM | 0:c3af3416e383 | 335 | teapotPacket[8] = 0; |
TUATBM | 0:c3af3416e383 | 336 | teapotPacket[9] = 0; |
TUATBM | 0:c3af3416e383 | 337 | teapotPacket[10] = 0x00; |
TUATBM | 0:c3af3416e383 | 338 | teapotPacket[11] = 0x00; |
TUATBM | 0:c3af3416e383 | 339 | teapotPacket[12] = '\r'; |
TUATBM | 0:c3af3416e383 | 340 | teapotPacket[13] = '\n'; |
TUATBM | 0:c3af3416e383 | 341 | |
TUATBM | 0:c3af3416e383 | 342 | } |