Mohamed Ashraf / MPU6050-DMP

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   MPU6050DMPV2

MyMPU6050.h@13:0fc0f858fe0c, 2020-03-18 (annotated)

Committer:
mohamedashraf
Date:
Wed Mar 18 21:53:16 2020 +0000
Revision:
13:0fc0f858fe0c
Parent:
12:775e1464e042 
dmp angles code

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 9:ac76b5b24b18 12
amandaghassaei 7:07f1f975429a 13 class MyMPU6050 {
amandaghassaei 6:748bae310e1e 14
amandaghassaei 6:748bae310e1e 15 public:
amandaghassaei 6:748bae310e1e 16
amandaghassaei 7:07f1f975429a 17 MyMPU6050(PinName i2cSda, PinName i2cScl, PinName interrupt) : mpu(i2cSda, i2cScl), checkpin(interrupt){
amandaghassaei 9:ac76b5b24b18 18 resetTheta();
amandaghassaei 8:96fa92188874 19 }
amandaghassaei 8:96fa92188874 20
amandaghassaei 8:96fa92188874 21 void setPC(Serial *pc){
amandaghassaei 8:96fa92188874 22 _pc = pc;
amandaghassaei 6:748bae310e1e 23 }
amandaghassaei 6:748bae310e1e 24
mohamedashraf 13:0fc0f858fe0c 25 float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
mohamedashraf 13:0fc0f858fe0c 26
mohamedashraf 13:0fc0f858fe0c 27
amandaghassaei 6:748bae310e1e 28 void loop() {
amandaghassaei 6:748bae310e1e 29 // if programming failed, don't try to do anything
amandaghassaei 6:748bae310e1e 30 if (!dmpReady) return;
amandaghassaei 10:6167cc7e7d60 31
amandaghassaei 10:6167cc7e7d60 32 // _pc->printf("dmp ready\n");
mohamedashraf 13:0fc0f858fe0c 33 mpuInterrupt=true;
amandaghassaei 6:748bae310e1e 34 // wait for MPU interrupt or extra packet(s) available
amandaghassaei 6:748bae310e1e 35 if (!mpuInterrupt && fifoCount < packetSize) {
amandaghassaei 6:748bae310e1e 36 return;
amandaghassaei 6:748bae310e1e 37 // other program behavior stuff here
amandaghassaei 6:748bae310e1e 38 // .
amandaghassaei 6:748bae310e1e 39 // .
amandaghassaei 6:748bae310e1e 40 // .
amandaghassaei 6:748bae310e1e 41 // if you are really paranoid you can frequently test in between other
amandaghassaei 6:748bae310e1e 42 // stuff to see if mpuInterrupt is true, and if so, "break;" from the
amandaghassaei 6:748bae310e1e 43 // while() loop to immediately process the MPU data
amandaghassaei 6:748bae310e1e 44 // .
amandaghassaei 6:748bae310e1e 45 // .
amandaghassaei 6:748bae310e1e 46 // .
amandaghassaei 6:748bae310e1e 47 }
amandaghassaei 10:6167cc7e7d60 48
amandaghassaei 10:6167cc7e7d60 49 // _pc->printf("mpu interrupt \n");
amandaghassaei 10:6167cc7e7d60 50
amandaghassaei 6:748bae310e1e 51 // reset interrupt flag and get INT_STATUS byte
amandaghassaei 6:748bae310e1e 52 mpuInterrupt = false;
amandaghassaei 6:748bae310e1e 53 mpuIntStatus = mpu.getIntStatus();
amandaghassaei 6:748bae310e1e 54
amandaghassaei 6:748bae310e1e 55 // get current FIFO count
amandaghassaei 6:748bae310e1e 56 fifoCount = mpu.getFIFOCount();
amandaghassaei 6:748bae310e1e 57
amandaghassaei 6:748bae310e1e 58 // check for overflow (this should never happen unless our code is too inefficient)
amandaghassaei 6:748bae310e1e 59 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
amandaghassaei 6:748bae310e1e 60 // reset so we can continue cleanly
amandaghassaei 6:748bae310e1e 61 mpu.resetFIFO();
amandaghassaei 11:feab67f916fe 62 // _pc->printf("FIFO overflow!\n");
amandaghassaei 6:748bae310e1e 63
amandaghassaei 6:748bae310e1e 64 // otherwise, check for DMP data ready interrupt (this should happen frequently)
amandaghassaei 6:748bae310e1e 65 } else if (mpuIntStatus & 0x02) {
amandaghassaei 6:748bae310e1e 66 // wait for correct available data length, should be a VERY short wait
amandaghassaei 6:748bae310e1e 67 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
amandaghassaei 6:748bae310e1e 68
amandaghassaei 6:748bae310e1e 69 // read a packet from FIFO
amandaghassaei 6:748bae310e1e 70 mpu.getFIFOBytes(fifoBuffer, packetSize);
amandaghassaei 6:748bae310e1e 71
amandaghassaei 6:748bae310e1e 72 // track FIFO count here in case there is > 1 packet available
amandaghassaei 6:748bae310e1e 73 // (this lets us immediately read more without waiting for an interrupt)
amandaghassaei 6:748bae310e1e 74 fifoCount -= packetSize;
amandaghassaei 6:748bae310e1e 75
amandaghassaei 6:748bae310e1e 76 // display Euler angles in degrees
amandaghassaei 6:748bae310e1e 77 mpu.dmpGetQuaternion(&q, fifoBuffer);
amandaghassaei 6:748bae310e1e 78 mpu.dmpGetGravity(&gravity, &q);
amandaghassaei 6:748bae310e1e 79 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
mohamedashraf 13:0fc0f858fe0c 80 //mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
amandaghassaei 9:ac76b5b24b18 81 float newTheta;
amandaghassaei 9:ac76b5b24b18 82 if (ypr[1]>0) newTheta = ypr[2];
amandaghassaei 9:ac76b5b24b18 83 else if (ypr[2]<0) newTheta = -M_PI-ypr[2];
amandaghassaei 9:ac76b5b24b18 84 else newTheta = M_PI-ypr[2];
amandaghassaei 9:ac76b5b24b18 85
amandaghassaei 9:ac76b5b24b18 86 mpu.dmpGetGyro(gyroData, fifoBuffer);
amandaghassaei 11:feab67f916fe 87 float newDTheta = -gyroData[2]/180.0*M_PI;
amandaghassaei 9:ac76b5b24b18 88
amandaghassaei 9:ac76b5b24b18 89 if (!thetaInitFlag && abs(newTheta-theta)>0.3) {
amandaghassaei 10:6167cc7e7d60 90 // _pc->printf("IMU interrupt clash\n");
amandaghassaei 9:ac76b5b24b18 91 mpu.resetFIFO();
amandaghassaei 9:ac76b5b24b18 92 } else {
amandaghassaei 9:ac76b5b24b18 93 thetaInitFlag = false;
amandaghassaei 9:ac76b5b24b18 94 theta = newTheta;
amandaghassaei 9:ac76b5b24b18 95 dtheta = newDTheta;
amandaghassaei 10:6167cc7e7d60 96 // _pc->printf("loop %f\n", theta);
amandaghassaei 9:ac76b5b24b18 97 }
amandaghassaei 8:96fa92188874 98
amandaghassaei 9:ac76b5b24b18 99
amandaghassaei 9:ac76b5b24b18 100
amandaghassaei 6:748bae310e1e 101 }
amandaghassaei 6:748bae310e1e 102 }
amandaghassaei 6:748bae310e1e 103
amandaghassaei 6:748bae310e1e 104 void dmpDataReady() {
amandaghassaei 6:748bae310e1e 105 mpuInterrupt = true;
amandaghassaei 6:748bae310e1e 106 }
amandaghassaei 6:748bae310e1e 107
amandaghassaei 6:748bae310e1e 108 float getTheta(){
amandaghassaei 10:6167cc7e7d60 109 // _pc->printf("%f\n", theta);
amandaghassaei 6:748bae310e1e 110 return theta;
amandaghassaei 6:748bae310e1e 111 }
amandaghassaei 6:748bae310e1e 112
amandaghassaei 7:07f1f975429a 113 float getDTheta(){
amandaghassaei 6:748bae310e1e 114 return dtheta;
amandaghassaei 6:748bae310e1e 115 }
amandaghassaei 8:96fa92188874 116
amandaghassaei 8:96fa92188874 117 void disable(){
amandaghassaei 8:96fa92188874 118 dmpReady = false;
amandaghassaei 8:96fa92188874 119 mpuInterrupt = false;
amandaghassaei 8:96fa92188874 120 checkpin.rise(NULL);
amandaghassaei 8:96fa92188874 121 }
amandaghassaei 8:96fa92188874 122
amandaghassaei 10:6167cc7e7d60 123 void disableInterrupt(){
amandaghassaei 10:6167cc7e7d60 124 mpuInterrupt = false;
amandaghassaei 10:6167cc7e7d60 125 checkpin.rise(NULL);
amandaghassaei 10:6167cc7e7d60 126 }
amandaghassaei 10:6167cc7e7d60 127
amandaghassaei 8:96fa92188874 128 void enable(){
amandaghassaei 8:96fa92188874 129 setup();
amandaghassaei 8:96fa92188874 130 }
amandaghassaei 10:6167cc7e7d60 131
amandaghassaei 10:6167cc7e7d60 132 void enableInterrupt(){
amandaghassaei 10:6167cc7e7d60 133 checkpin.rise(this, &MyMPU6050::dmpDataReady);
amandaghassaei 10:6167cc7e7d60 134 }
amandaghassaei 6:748bae310e1e 135
amandaghassaei 6:748bae310e1e 136 private:
amandaghassaei 7:07f1f975429a 137
amandaghassaei 8:96fa92188874 138 Serial *_pc;
amandaghassaei 8:96fa92188874 139
amandaghassaei 6:748bae310e1e 140 MPU6050 mpu;
amandaghassaei 6:748bae310e1e 141
amandaghassaei 6:748bae310e1e 142 bool dmpReady; // set true if DMP init was successful
amandaghassaei 6:748bae310e1e 143 uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
amandaghassaei 6:748bae310e1e 144 uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
amandaghassaei 6:748bae310e1e 145 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
amandaghassaei 6:748bae310e1e 146 uint16_t fifoCount; // count of all bytes currently in FIFO
amandaghassaei 6:748bae310e1e 147 uint8_t fifoBuffer[64]; // FIFO storage buffer
amandaghassaei 6:748bae310e1e 148
amandaghassaei 6:748bae310e1e 149 // orientation/motion vars
amandaghassaei 6:748bae310e1e 150 Quaternion q; // [w, x, y, z] quaternion container
amandaghassaei 6:748bae310e1e 151 VectorFloat gravity; // [x, y, z] gravity vector
amandaghassaei 6:748bae310e1e 152 int16_t gyroData[3];
amandaghassaei 10:6167cc7e7d60 153 volatile float theta;
amandaghassaei 9:ac76b5b24b18 154 bool thetaInitFlag;
amandaghassaei 10:6167cc7e7d60 155 volatile float dtheta;
amandaghassaei 6:748bae310e1e 156
amandaghassaei 6:748bae310e1e 157 InterruptIn checkpin;
amandaghassaei 6:748bae310e1e 158 volatile bool mpuInterrupt;
amandaghassaei 6:748bae310e1e 159
amandaghassaei 7:07f1f975429a 160
amandaghassaei 9:ac76b5b24b18 161 void resetTheta(){
amandaghassaei 9:ac76b5b24b18 162 theta = 0;
amandaghassaei 9:ac76b5b24b18 163 dtheta = 0;
amandaghassaei 9:ac76b5b24b18 164 thetaInitFlag = true;
amandaghassaei 9:ac76b5b24b18 165 }
amandaghassaei 9:ac76b5b24b18 166
amandaghassaei 6:748bae310e1e 167 void setup() {
amandaghassaei 6:748bae310e1e 168
amandaghassaei 9:ac76b5b24b18 169 resetTheta();
amandaghassaei 6:748bae310e1e 170
amandaghassaei 6:748bae310e1e 171 dmpReady = false;
amandaghassaei 6:748bae310e1e 172 mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
amandaghassaei 6:748bae310e1e 173
amandaghassaei 6:748bae310e1e 174 // initialize device
amandaghassaei 6:748bae310e1e 175 // pc.printf("Initializing I2C devices...\r\n");
amandaghassaei 6:748bae310e1e 176 mpu.initialize();
amandaghassaei 6:748bae310e1e 177
amandaghassaei 6:748bae310e1e 178 // verify connection
amandaghassaei 6:748bae310e1e 179 // pc.printf("Testing device connections...\r\n");
amandaghassaei 6:748bae310e1e 180 mpu.testConnection();//if() pc.printf("MPU6050 connection successful\r\n");
amandaghassaei 6:748bae310e1e 181 // else pc.printf("MPU6050 connection failed\r\n");
amandaghassaei 6:748bae310e1e 182
amandaghassaei 6:748bae310e1e 183 // load and configure the DMP
amandaghassaei 6:748bae310e1e 184 // pc.printf("Initializing DMP...\r\n");
amandaghassaei 6:748bae310e1e 185 devStatus = mpu.dmpInitialize();
amandaghassaei 6:748bae310e1e 186
amandaghassaei 6:748bae310e1e 187 // make sure it worked (returns 0 if so)
amandaghassaei 6:748bae310e1e 188 if (devStatus == 0) {
amandaghassaei 6:748bae310e1e 189 // turn on the DMP, now that it's ready
amandaghassaei 6:748bae310e1e 190 // pc.printf("Enabling DMP...\r\n");
amandaghassaei 6:748bae310e1e 191 mpu.setDMPEnabled(true);
amandaghassaei 6:748bae310e1e 192
amandaghassaei 6:748bae310e1e 193 // enable Arduino interrupt detection
amandaghassaei 6:748bae310e1e 194 // pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n");
amandaghassaei 6:748bae310e1e 195 checkpin.rise(this, &MyMPU6050::dmpDataReady);
amandaghassaei 6:748bae310e1e 196
amandaghassaei 6:748bae310e1e 197 mpuIntStatus = mpu.getIntStatus();
amandaghassaei 6:748bae310e1e 198
amandaghassaei 6:748bae310e1e 199 // set our DMP Ready flag so the main loop() function knows it's okay to use it
amandaghassaei 6:748bae310e1e 200 // pc.printf("DMP ready! Waiting for first interrupt...\r\n");
amandaghassaei 6:748bae310e1e 201 dmpReady = true;
amandaghassaei 6:748bae310e1e 202
amandaghassaei 6:748bae310e1e 203 // get expected DMP packet size for later comparison
amandaghassaei 6:748bae310e1e 204 packetSize = mpu.dmpGetFIFOPacketSize();
amandaghassaei 6:748bae310e1e 205 } else {
amandaghassaei 6:748bae310e1e 206 // ERROR!
amandaghassaei 6:748bae310e1e 207 // 1 = initial memory load failed
amandaghassaei 6:748bae310e1e 208 // 2 = DMP configuration updates failed
amandaghassaei 6:748bae310e1e 209 // (if it's going to break, usually the code will be 1)
amandaghassaei 6:748bae310e1e 210
amandaghassaei 6:748bae310e1e 211 // pc.printf("DDMP Initialization failed (code ");
amandaghassaei 6:748bae310e1e 212 // pc.printf("%d", devStatus);
amandaghassaei 6:748bae310e1e 213 // pc.printf(")\r\n");
amandaghassaei 6:748bae310e1e 214 }
amandaghassaei 6:748bae310e1e 215
amandaghassaei 6:748bae310e1e 216 }
amandaghassaei 6:748bae310e1e 217 };