sfas
Dependents: Bracky-MPU6050-DMP
Fork of MPU6050 by
Diff: MyMPU6050.h
- Revision:
- 6:748bae310e1e
- Child:
- 7:07f1f975429a
diff -r 7d1bf3ce0053 -r 748bae310e1e MyMPU6050.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MyMPU6050.h Thu Dec 03 02:23:23 2015 +0000 @@ -0,0 +1,160 @@ +// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0) +// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net> +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2012 Jeff Rowberg +*/ + +#include "I2Cdev.h" +#include "MPU6050_6Axis_MotionApps20.h" +#include "CommDelegate.h" + +class MyMPU6050: public CommDelegate { + + public: + + MyMPU6050(PinName i2cSda, PinName i2cScl) : mpu(i2cSda, i2cScl), checkpin(p11){ + this->setup(); + } + + void loop() { + // if programming failed, don't try to do anything + if (!dmpReady) return; + + // wait for MPU interrupt or extra packet(s) available + if (!mpuInterrupt && fifoCount < packetSize) { + return; + // other program behavior stuff here + // . + // . + // . + // if you are really paranoid you can frequently test in between other + // stuff to see if mpuInterrupt is true, and if so, "break;" from the + // while() loop to immediately process the MPU data + // . + // . + // . + } + + // reset interrupt flag and get INT_STATUS byte + mpuInterrupt = false; + mpuIntStatus = mpu.getIntStatus(); + + // get current FIFO count + fifoCount = mpu.getFIFOCount(); + + // check for overflow (this should never happen unless our code is too inefficient) + if ((mpuIntStatus & 0x10) || fifoCount == 1024) { + // reset so we can continue cleanly + mpu.resetFIFO(); + //Serial.println(F("FIFO overflow!")); + + // otherwise, check for DMP data ready interrupt (this should happen frequently) + } else if (mpuIntStatus & 0x02) { + // wait for correct available data length, should be a VERY short wait + while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); + + // read a packet from FIFO + mpu.getFIFOBytes(fifoBuffer, packetSize); + + // track FIFO count here in case there is > 1 packet available + // (this lets us immediately read more without waiting for an interrupt) + fifoCount -= packetSize; + + // display Euler angles in degrees + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); + mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); + theta = ypr[2]; + mpu.dmpGetGyro(gyroData, fifoBuffer); + dtheta = gyroData[2]; + } + } + + void dmpDataReady() { + mpuInterrupt = true; + } + + float getTheta(){ + return theta; + } + + float getDtheta(){ + return dtheta; + } + + private: + + MPU6050 mpu; + + bool dmpReady; // set true if DMP init was successful + uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU + uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) + uint16_t packetSize; // expected DMP packet size (default is 42 bytes) + uint16_t fifoCount; // count of all bytes currently in FIFO + uint8_t fifoBuffer[64]; // FIFO storage buffer + + // orientation/motion vars + Quaternion q; // [w, x, y, z] quaternion container + VectorFloat gravity; // [x, y, z] gravity vector + float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector + int16_t gyroData[3]; + float theta; + float dtheta; + + InterruptIn checkpin; + volatile bool mpuInterrupt; + + void setup() { + + theta = 0; + dtheta = 0; + + dmpReady = false; + mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high + + // initialize device +// pc.printf("Initializing I2C devices...\r\n"); + mpu.initialize(); + + // verify connection +// pc.printf("Testing device connections...\r\n"); + mpu.testConnection();//if() pc.printf("MPU6050 connection successful\r\n"); +// else pc.printf("MPU6050 connection failed\r\n"); + + // load and configure the DMP +// pc.printf("Initializing DMP...\r\n"); + devStatus = mpu.dmpInitialize(); + + // make sure it worked (returns 0 if so) + if (devStatus == 0) { + // turn on the DMP, now that it's ready +// pc.printf("Enabling DMP...\r\n"); + mpu.setDMPEnabled(true); + + // enable Arduino interrupt detection +// pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n"); + checkpin.rise(this, &MyMPU6050::dmpDataReady); + + mpuIntStatus = mpu.getIntStatus(); + + // set our DMP Ready flag so the main loop() function knows it's okay to use it +// pc.printf("DMP ready! Waiting for first interrupt...\r\n"); + dmpReady = true; + + // get expected DMP packet size for later comparison + packetSize = mpu.dmpGetFIFOPacketSize(); + } else { + // ERROR! + // 1 = initial memory load failed + // 2 = DMP configuration updates failed + // (if it's going to break, usually the code will be 1) + +// pc.printf("DDMP Initialization failed (code "); +// pc.printf("%d", devStatus); +// pc.printf(")\r\n"); + } + + } +}; \ No newline at end of file