File content as of revision 0:dceb852b19f3:

//Copied MPU6050.h and MPU6050.cpp from another library (otherwise MPU gets stuck randomly)

/*** THIS IS THE BEST VERSION OF DMP CODE
    This code demonstrates how to access the MPU6050 IMU and fix the 
    connection errors that occur, using a hardware hack.
    It requires two hardware fixes: A transistor (PTE5) and an extra GPIO pin (PTE23) connected
    to the SDA line.
    Error 1: The SDA line gets stuck on ground.
    Solution 1: Use the transistor to raise the SDA voltage off of ground. This resets the I2C bus. (line 71)
    Error 2: The SDA line gets stuck on 3.3V
    Solution 2: Use the GPIO line to pull the SDA voltage down a bit to reset the I2C bus. (line 96)
    (I am currently trying to find a software solution for the above hack)
    
    This code works for using the DMP. (See "DMP" for implementation that can do raw accel data)
    It appears all connection issues have been fixed.
    Next step: make code neater (in progress)
    
    LEARNED: Do not need the PTE23 line. Only fix necessary is the transistor (PTE5)
***/

#include "mbed.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "robot.h"


//******* MPU DECLARATION THINGS *****************//
int dmp_test =1;    //if this is 0, get raw MPU values
                 //if this is 1, get DMP values
            
int16_t ax, ay, az;
int16_t gx, gy, gz;

//******* DMP things ****************//
// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was succesfful
uint8_t mpuIntStatus;   // holds interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success)
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
    
    
uint8_t MPU_CONNECTION;
        
// orientation/motion vars
Quaternion q;           // [w,x,y,z]    quaternion container
VectorInt16 aa;         // [x,y,z]      accel sensor measurements
VectorInt16 aaReal;     // [x,y,z]      gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x,y,z]      world-frame accel sensor measurements
VectorFloat gravity;    // [x,y,z]      gravity vector
float euler[3];         // [psi,theta,phi]  Euler angle container
float ypr[3];           // [yaw,pitch,roll] yaw/pitch/roll container [rad]. Multiply by 180, divide by PI for [deg]
InterruptIn checkpin(PTD7); //interrupt
//*** Interrupt Detection Routine ***//
volatile bool mpuInterrupt = false; //indicates whether interrupt pin has gone high
void dmpDataReady(){
    mpuInterrupt = true;
}
        
//****** END MPU DECLARATION ********************//


int test_dmp();    //this wraps up the code req'd to start the DMP
void start_dmp(MPU6050 mpu);    //this will get the DMP ready
void update_dmp(MPU6050 mpu);   //call this frequently

//*********************FUNCTIONS***************************************************************//
int test_dmp(){ //returns 1 if ok (right now, it doesn't return at all if there is a problem    
    /// This function tests the I2C port for the MPU6050 and resets it if it is not working 
    //NOTE: when the I2C connection is not successful, calling initialize() will cause the board to freeze
    DigitalOut reset_pin(PTE5); //Transistor to pull up SDA line
    reset_pin = 0;  //start the transistor in OFF mode
    //int count = 0;
    DigitalIn sample_1(PTE0); //(this pin will be used as the SDA pin when an MPU6050 object is declared
    int temp = sample_1;    //Sample what is happening on the SDA line to guess what the solution is.
    myled = 1;      //turn OFF LED. This will turn on again when the MPU is connected.
    //bt.baud(baudRate);
    DigitalOut reset_1(PTE0);//change the SDA pin to output.
    reset_1 = 0;
    if(temp == 1){  //if SDA line starts high, unstick by pulling it low
        bt.printf("\n\rSetting up I2C connection...");
        reset_1 = 0; //SDA pin = low
        wait_ms(100);
        MPU6050 *mpu2 = new MPU6050; //create a temporary MPU connection for testing
        mpu2->reset();
        //bt.baud(baudRate);
        while(mpu2->testConnection() == 0){
           delete mpu2; //delete mpu2 so I can use PTE0 as a GPIO again
           bt.baud(baudRate); //you must reset baud rate whenever a new MPU6050 is declared
           DigitalOut sda_pinx(PTE0);   
           sda_pinx = 0;        //set this to 0 to pull the SDA line down from 3.3V
           reset_pin = 0;       //make sure transistor = OFF
           bt.printf("\n\rRetry");
           wait_ms(50);
           MPU6050 *mpu2 = new MPU6050;
           wait_ms(500);
           if(mpu2->testConnection() == 0)
           {
               reset_pin = 1;
               wait_ms(50);
               reset_pin = 0;
           }
        }
        delete mpu2;    //free the memory allocated to mpu2 (this is important)
    }else{
        bt.printf("\n\rSetting up I2C connection...");
        reset_1 = 1;
        reset_pin = 1;
        wait_ms(20);
        reset_pin = 0;
        reset_1 = 0;
        wait_ms(20);
    }
    reset_pin = 0;
    DigitalIn not_used(PTE23);
    myled = 0;  //turn ON LED
    return 1;
}


void start_dmp(MPU6050 mpu1){    //this will get the DMP ready
    //initialize device
    mpu1.reset();
    bt.printf("\n\rInitializing I2C devices...");
    devStatus = mpu1.dmpInitialize();
    //insert gyro offsets here// Write a calibration algorithm in the future
    mpu1.setXGyroOffset(-31); //800/25=32 //-31 is the largest offset available
    mpu1.setYGyroOffset(-183/25);
    mpu1.setZGyroOffset(80/25);
    mpu1.setXAccelOffset(0);     //the accel offsets don't do anything
    mpu1.setYAccelOffset(0);
    mpu1.setZAccelOffset(0);
    
    
    if(devStatus == 0){     //this means initialize was successful
        //turn on DMP
        bt.printf("\n\rEnabling DMP");
        mpu1.setDMPEnabled(true);
        
        // enable  interrupt detection
        bt.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n");
        checkpin.rise(&dmpDataReady);
    
        mpuIntStatus = mpu1.getIntStatus();
        
        //set the DMP ready flag so main loop knows when it is ok to use
        bt.printf("DMP ready! Waiting for first interrupt");
        dmpReady = true;
        
        //get expected DMP packet size
        packetSize = mpu1.dmpGetFIFOPacketSize();
    }else{
        bt.printf("\n\rDMP Initialization failed");
        bt.printf("\t%d",devStatus);
        wait_ms(1000);
    }
}
void update_dmp(MPU6050 mpu1){
    
    if (!dmpReady) return;
    //while (!mpuInterrupt && fifoCount < packetSize) {
        // 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;   //this resets the interrupt flag
    mpuIntStatus = mpu1.getIntStatus();
    
    //get current FIFO count;
    fifoCount = mpu1.getFIFOCount();
    
    //check for overflow (only happens if your code sucks)
    if((mpuIntStatus & 0x10) || fifoCount == 1024){
        //reset so we can continue cleanly
        mpu1.resetFIFO();
        bt.printf("\n\rFIFO overflow");
    } else if (mpuIntStatus & 0x02){
        //wait for correct available data length (should be very short)
        while (fifoCount < packetSize) fifoCount = mpu1.getFIFOCount();

        //read a packet from FIFO
        mpu1.getFIFOBytes(fifoBuffer, packetSize);
        
        //track FIFO count here in the case there is > 1 packet available
        //(allows us to immediately read more w/o waiting for interrupt)
        fifoCount -= packetSize;
        
        mpu1.dmpGetQuaternion(&q,fifoBuffer);
        mpu1.dmpGetGravity(&gravity, &q);
        mpu1.dmpGetYawPitchRoll(ypr,&q,&gravity);
        #ifdef PRINT_GYR
        bt.printf("\n\rypr\t %f\t %f\t %f",ypr[0]*180/PI,ypr[1]*180/PI,ypr[2]*180/PI);
        #endif
        
        mpu1.dmpGetAccel(&aa, fifoBuffer);
        mpu1.dmpGetLinearAccel(&aaReal, &aa, &gravity);
        #ifdef PRINT_ACC
        bt.printf("\n\rareal\t%d\t%d\t%d",aaReal.x,aaReal.y,aaReal.z);
        #endif
   }     
}