航空研究会 / MPU6050_2

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?

UserRevisionLine numberNew 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 }