Amanda Ghassaei / MPU6050-DMP

sfas

Dependents:   Bracky-MPU6050-DMP

Fork of MPU6050 by Shundo Kishi

MyMPU6050.h@7:07f1f975429a, 2015-12-03 (annotated)

Committer:
amandaghassaei
Date:
Thu Dec 03 23:55:33 2015 +0000
Revision:
7:07f1f975429a
Parent:
6:748bae310e1e
Child:
8:96fa92188874
theta offset;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
amandaghassaei 6:748bae310e1e 1 // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
amandaghassaei 6:748bae310e1e 2 // 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
amandaghassaei 6:748bae310e1e 3 /* ============================================
amandaghassaei 6:748bae310e1e 4 I2Cdev device library code is placed under the MIT license
amandaghassaei 6:748bae310e1e 5 Copyright (c) 2012 Jeff Rowberg
amandaghassaei 6:748bae310e1e 6 */
amandaghassaei 6:748bae310e1e 7
amandaghassaei 6:748bae310e1e 8 #include "I2Cdev.h"
amandaghassaei 6:748bae310e1e 9 #include "MPU6050_6Axis_MotionApps20.h"
amandaghassaei 7:07f1f975429a 10 # define M_PI 3.14159265358979323846
amandaghassaei 6:748bae310e1e 11
amandaghassaei 7:07f1f975429a 12 class MyMPU6050 {
amandaghassaei 6:748bae310e1e 13
amandaghassaei 6:748bae310e1e 14 public:
amandaghassaei 6:748bae310e1e 15
amandaghassaei 7:07f1f975429a 16 MyMPU6050(PinName i2cSda, PinName i2cScl, PinName interrupt) : mpu(i2cSda, i2cScl), checkpin(interrupt){
amandaghassaei 6:748bae310e1e 17 this->setup();
amandaghassaei 6:748bae310e1e 18 }
amandaghassaei 6:748bae310e1e 19
amandaghassaei 6:748bae310e1e 20 void loop() {
amandaghassaei 6:748bae310e1e 21 // if programming failed, don't try to do anything
amandaghassaei 6:748bae310e1e 22 if (!dmpReady) return;
amandaghassaei 6:748bae310e1e 23
amandaghassaei 6:748bae310e1e 24 // wait for MPU interrupt or extra packet(s) available
amandaghassaei 6:748bae310e1e 25 if (!mpuInterrupt && fifoCount < packetSize) {
amandaghassaei 6:748bae310e1e 26 return;
amandaghassaei 6:748bae310e1e 27 // other program behavior stuff here
amandaghassaei 6:748bae310e1e 28 // .
amandaghassaei 6:748bae310e1e 29 // .
amandaghassaei 6:748bae310e1e 30 // .
amandaghassaei 6:748bae310e1e 31 // if you are really paranoid you can frequently test in between other
amandaghassaei 6:748bae310e1e 32 // stuff to see if mpuInterrupt is true, and if so, "break;" from the
amandaghassaei 6:748bae310e1e 33 // while() loop to immediately process the MPU data
amandaghassaei 6:748bae310e1e 34 // .
amandaghassaei 6:748bae310e1e 35 // .
amandaghassaei 6:748bae310e1e 36 // .
amandaghassaei 6:748bae310e1e 37 }
amandaghassaei 6:748bae310e1e 38
amandaghassaei 6:748bae310e1e 39 // reset interrupt flag and get INT_STATUS byte
amandaghassaei 6:748bae310e1e 40 mpuInterrupt = false;
amandaghassaei 6:748bae310e1e 41 mpuIntStatus = mpu.getIntStatus();
amandaghassaei 6:748bae310e1e 42
amandaghassaei 6:748bae310e1e 43 // get current FIFO count
amandaghassaei 6:748bae310e1e 44 fifoCount = mpu.getFIFOCount();
amandaghassaei 6:748bae310e1e 45
amandaghassaei 6:748bae310e1e 46 // check for overflow (this should never happen unless our code is too inefficient)
amandaghassaei 6:748bae310e1e 47 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
amandaghassaei 6:748bae310e1e 48 // reset so we can continue cleanly
amandaghassaei 6:748bae310e1e 49 mpu.resetFIFO();
amandaghassaei 6:748bae310e1e 50 //Serial.println(F("FIFO overflow!"));
amandaghassaei 6:748bae310e1e 51
amandaghassaei 6:748bae310e1e 52 // otherwise, check for DMP data ready interrupt (this should happen frequently)
amandaghassaei 6:748bae310e1e 53 } else if (mpuIntStatus & 0x02) {
amandaghassaei 6:748bae310e1e 54 // wait for correct available data length, should be a VERY short wait
amandaghassaei 6:748bae310e1e 55 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
amandaghassaei 6:748bae310e1e 56
amandaghassaei 6:748bae310e1e 57 // read a packet from FIFO
amandaghassaei 6:748bae310e1e 58 mpu.getFIFOBytes(fifoBuffer, packetSize);
amandaghassaei 6:748bae310e1e 59
amandaghassaei 6:748bae310e1e 60 // track FIFO count here in case there is > 1 packet available
amandaghassaei 6:748bae310e1e 61 // (this lets us immediately read more without waiting for an interrupt)
amandaghassaei 6:748bae310e1e 62 fifoCount -= packetSize;
amandaghassaei 6:748bae310e1e 63
amandaghassaei 6:748bae310e1e 64 // display Euler angles in degrees
amandaghassaei 6:748bae310e1e 65 mpu.dmpGetQuaternion(&q, fifoBuffer);
amandaghassaei 6:748bae310e1e 66 mpu.dmpGetGravity(&gravity, &q);
amandaghassaei 6:748bae310e1e 67 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
amandaghassaei 6:748bae310e1e 68 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
amandaghassaei 7:07f1f975429a 69 if (ypr[1]>0) theta = ypr[2];
amandaghassaei 7:07f1f975429a 70 else if (ypr[2]<0) theta = -M_PI-ypr[2];
amandaghassaei 7:07f1f975429a 71 else theta = M_PI-ypr[2];
amandaghassaei 7:07f1f975429a 72
amandaghassaei 7:07f1f975429a 73 // theta = ypr[1];
amandaghassaei 7:07f1f975429a 74
amandaghassaei 6:748bae310e1e 75 mpu.dmpGetGyro(gyroData, fifoBuffer);
amandaghassaei 6:748bae310e1e 76 dtheta = gyroData[2];
amandaghassaei 6:748bae310e1e 77 }
amandaghassaei 6:748bae310e1e 78 }
amandaghassaei 6:748bae310e1e 79
amandaghassaei 6:748bae310e1e 80 void dmpDataReady() {
amandaghassaei 6:748bae310e1e 81 mpuInterrupt = true;
amandaghassaei 6:748bae310e1e 82 }
amandaghassaei 6:748bae310e1e 83
amandaghassaei 6:748bae310e1e 84 float getTheta(){
amandaghassaei 6:748bae310e1e 85 return theta;
amandaghassaei 6:748bae310e1e 86 }
amandaghassaei 6:748bae310e1e 87
amandaghassaei 7:07f1f975429a 88 float getDTheta(){
amandaghassaei 6:748bae310e1e 89 return dtheta;
amandaghassaei 6:748bae310e1e 90 }
amandaghassaei 6:748bae310e1e 91
amandaghassaei 6:748bae310e1e 92 private:
amandaghassaei 7:07f1f975429a 93
amandaghassaei 6:748bae310e1e 94 MPU6050 mpu;
amandaghassaei 6:748bae310e1e 95
amandaghassaei 6:748bae310e1e 96 bool dmpReady; // set true if DMP init was successful
amandaghassaei 6:748bae310e1e 97 uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
amandaghassaei 6:748bae310e1e 98 uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
amandaghassaei 6:748bae310e1e 99 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
amandaghassaei 6:748bae310e1e 100 uint16_t fifoCount; // count of all bytes currently in FIFO
amandaghassaei 6:748bae310e1e 101 uint8_t fifoBuffer[64]; // FIFO storage buffer
amandaghassaei 6:748bae310e1e 102
amandaghassaei 6:748bae310e1e 103 // orientation/motion vars
amandaghassaei 6:748bae310e1e 104 Quaternion q; // [w, x, y, z] quaternion container
amandaghassaei 6:748bae310e1e 105 VectorFloat gravity; // [x, y, z] gravity vector
amandaghassaei 6:748bae310e1e 106 float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
amandaghassaei 6:748bae310e1e 107 int16_t gyroData[3];
amandaghassaei 6:748bae310e1e 108 float theta;
amandaghassaei 6:748bae310e1e 109 float dtheta;
amandaghassaei 6:748bae310e1e 110
amandaghassaei 6:748bae310e1e 111 InterruptIn checkpin;
amandaghassaei 6:748bae310e1e 112 volatile bool mpuInterrupt;
amandaghassaei 6:748bae310e1e 113
amandaghassaei 7:07f1f975429a 114
amandaghassaei 6:748bae310e1e 115 void setup() {
amandaghassaei 6:748bae310e1e 116
amandaghassaei 6:748bae310e1e 117 theta = 0;
amandaghassaei 6:748bae310e1e 118 dtheta = 0;
amandaghassaei 6:748bae310e1e 119
amandaghassaei 6:748bae310e1e 120 dmpReady = false;
amandaghassaei 6:748bae310e1e 121 mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
amandaghassaei 6:748bae310e1e 122
amandaghassaei 6:748bae310e1e 123 // initialize device
amandaghassaei 6:748bae310e1e 124 // pc.printf("Initializing I2C devices...\r\n");
amandaghassaei 6:748bae310e1e 125 mpu.initialize();
amandaghassaei 6:748bae310e1e 126
amandaghassaei 6:748bae310e1e 127 // verify connection
amandaghassaei 6:748bae310e1e 128 // pc.printf("Testing device connections...\r\n");
amandaghassaei 6:748bae310e1e 129 mpu.testConnection();//if() pc.printf("MPU6050 connection successful\r\n");
amandaghassaei 6:748bae310e1e 130 // else pc.printf("MPU6050 connection failed\r\n");
amandaghassaei 6:748bae310e1e 131
amandaghassaei 6:748bae310e1e 132 // load and configure the DMP
amandaghassaei 6:748bae310e1e 133 // pc.printf("Initializing DMP...\r\n");
amandaghassaei 6:748bae310e1e 134 devStatus = mpu.dmpInitialize();
amandaghassaei 6:748bae310e1e 135
amandaghassaei 6:748bae310e1e 136 // make sure it worked (returns 0 if so)
amandaghassaei 6:748bae310e1e 137 if (devStatus == 0) {
amandaghassaei 6:748bae310e1e 138 // turn on the DMP, now that it's ready
amandaghassaei 6:748bae310e1e 139 // pc.printf("Enabling DMP...\r\n");
amandaghassaei 6:748bae310e1e 140 mpu.setDMPEnabled(true);
amandaghassaei 6:748bae310e1e 141
amandaghassaei 6:748bae310e1e 142 // enable Arduino interrupt detection
amandaghassaei 6:748bae310e1e 143 // pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n");
amandaghassaei 6:748bae310e1e 144 checkpin.rise(this, &MyMPU6050::dmpDataReady);
amandaghassaei 6:748bae310e1e 145
amandaghassaei 6:748bae310e1e 146 mpuIntStatus = mpu.getIntStatus();
amandaghassaei 6:748bae310e1e 147
amandaghassaei 6:748bae310e1e 148 // set our DMP Ready flag so the main loop() function knows it's okay to use it
amandaghassaei 6:748bae310e1e 149 // pc.printf("DMP ready! Waiting for first interrupt...\r\n");
amandaghassaei 6:748bae310e1e 150 dmpReady = true;
amandaghassaei 6:748bae310e1e 151
amandaghassaei 6:748bae310e1e 152 // get expected DMP packet size for later comparison
amandaghassaei 6:748bae310e1e 153 packetSize = mpu.dmpGetFIFOPacketSize();
amandaghassaei 6:748bae310e1e 154 } else {
amandaghassaei 6:748bae310e1e 155 // ERROR!
amandaghassaei 6:748bae310e1e 156 // 1 = initial memory load failed
amandaghassaei 6:748bae310e1e 157 // 2 = DMP configuration updates failed
amandaghassaei 6:748bae310e1e 158 // (if it's going to break, usually the code will be 1)
amandaghassaei 6:748bae310e1e 159
amandaghassaei 6:748bae310e1e 160 // pc.printf("DDMP Initialization failed (code ");
amandaghassaei 6:748bae310e1e 161 // pc.printf("%d", devStatus);
amandaghassaei 6:748bae310e1e 162 // pc.printf(")\r\n");
amandaghassaei 6:748bae310e1e 163 }
amandaghassaei 6:748bae310e1e 164
amandaghassaei 6:748bae310e1e 165 }
amandaghassaei 6:748bae310e1e 166 };