航空研究会 / MPU6050_2

aaa

Dependencies:   ArduinoSerial I2Cdev2

Dependents:   AutoFlight2017_now2 AutoFlight2018_Control sbus_test_2018 Autoflight2018_sbusread ... more

MPU6050_DMP6.cpp@2:ff025b9c6c7c, 2018-09-18 (annotated)

Committer:
taknokolat
Date:
Tue Sep 18 05:53:06 2018 +0000
Revision:
2:ff025b9c6c7c
Parent:
0:c3af3416e383 
a

Who changed what in which revision?

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