File content as of revision 0:964eb6a2ef00:

//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 "DMP.h"


//******* MPU DECLARATION THINGS *****************//
int dmp_test = 1;    //if this is 0, get raw MPU values
                 //if this is 1, get DMP values

int acc_offset[3];  //this saves the offset for acceleration
 
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
bool dmpReady2 = false;  // set true if DMP init was succesfful
uint8_t mpuIntStatus2;   // holds interrupt status byte from MPU
uint8_t devStatus2;      // return status after each device operation (0 = success)
uint16_t packetSize2;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount2;     // count of all bytes currently in FIFO
uint8_t fifoBuffer2[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
InterruptIn checkpin2(PTD5); //PTD5
//*** Interrupt Detection Routine ***//
volatile bool mpuInterrupt = false; //indicates whether interrupt pin has gone high
void dmpDataReady(){
    mpuInterrupt = true;
}
volatile bool mpuInterrupt2 = false; //indicates whether interrupt pin has gone high
void dmpDataReady2(){
    mpuInterrupt2 = true;
}
        
//****** END MPU DECLARATION ********************//


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

//*********************FUNCTIONS***************************************************************//
int test_dmp(){ //returns 1 if ok (right now, it doesn't return at all if there is a problem    
    imuSwitch = 1;
    LED_OFF;
    Thread::wait(100);
    
    while(mpu.testConnection()==0)
    {
        imuSwitch = 0;
        Thread::wait(100);
        imuSwitch = 1;
        Thread::wait(100);
        bt.lock();
        bt.printf("Stuck on IMU1");
        bt.unlock();
    }
    imuSwitch = 1;
    LED_ON  //turn ON LED
    return 1;
}
int test_dmp2(){ //returns 1 if ok (right now, it doesn't return at all if there is a problem    
    imu2Switch = 1;
    LED_OFF;
    Thread::wait(100);
    
    while(mpu2.testConnection()==0)
    {
        imu2Switch = 0;
        Thread::wait(100);
        imu2Switch = 1;
        Thread::wait(100);
        bt.lock();
        bt.printf("Stuck on IMU2");
        bt.unlock();
    }
    imu2Switch = 1;
    LED_ON  //turn ON LED
    return 1;
}

void start_dmp(MPU6050 mpu1){    //this will get the DMP ready
    //initialize device
    mpu1.reset();
    DMP_DBG_MSG("\n\rInitializing I2C devices...")
    devStatus = mpu1.dmpInitialize();
    //insert gyro offsets here// Write a calibration algorithm in the future
    mpu1.setXGyroOffset(0); //800/25=32 //-31 is the largest offset available
    mpu1.setYGyroOffset(0);
    mpu1.setZGyroOffset(0);
    mpu1.setXAccelOffset(0);     //the accel offsets don't do anything
    mpu1.setYAccelOffset(0);
    mpu1.setZAccelOffset(0);
    
    mpu1.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
    
    if(devStatus == 0){     //this means initialize was successful
        //turn on DMP
        DMP_DBG_MSG("\n\rEnabling DMP")
        mpu1.setDMPEnabled(true);
        
        // enable  interrupt detection
        DMP_DBG_MSG("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
        DMP_DBG_MSG("DMP ready! Waiting for first interrupt")
        dmpReady = true;
        
        //get expected DMP packet size
        packetSize = mpu1.dmpGetFIFOPacketSize();
    }else{
        DMP_DBG_MSG("\n\rDMP Initialization failed")
        DMP_DBG_MSG("\t%d",devStatus)
        Thread::wait(1000);
    }
    acc_offset[0]=0;
    acc_offset[1]=0;
    acc_offset[2]=0;
    mpu.setDLPFMode(MPU6050_DLPF_BW_42);
}
void start_dmp2(MPU6051 mpu1){    //this will get the DMP ready
    //initialize device
    mpu2.reset();
    DMP_DBG_MSG("\n\rInitializing I2C devices...")
    devStatus2 = mpu2.dmpInitialize();
    //insert gyro offsets here// Write a calibration algorithm in the future
    mpu2.setXGyroOffset(0); //800/25=32 //-31 is the largest offset available
    mpu2.setYGyroOffset(0);
    mpu2.setZGyroOffset(0);
    mpu2.setXAccelOffset(0);     //the accel offsets don't do anything
    mpu2.setYAccelOffset(0);
    mpu2.setZAccelOffset(0);
    
    mpu2.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
    
    if(devStatus2 == 0){     //this means initialize was successful
        //turn on DMP
        DMP_DBG_MSG("\n\rEnabling DMP")
        mpu2.setDMPEnabled(true);
        
        // enable  interrupt detection
        DMP_DBG_MSG("Enabling interrupt detection (Arduino external interrupt 0)...\r\n")
        checkpin2.rise(&dmpDataReady2);
    
        mpuIntStatus2 = mpu2.getIntStatus();
        
        //set the DMP ready flag so main loop knows when it is ok to use
        DMP_DBG_MSG("DMP ready! Waiting for first interrupt")
        dmpReady2 = true;
        
        //get expected DMP packet size
        packetSize2 = mpu2.dmpGetFIFOPacketSize();
    }else{
        DMP_DBG_MSG("\n\rDMP Initialization failed")
        DMP_DBG_MSG("\t%d",devStatus2)
        Thread::wait(1000);
    }
    acc_offset[0]=0;
    acc_offset[1]=0;
    acc_offset[2]=0;
    mpu2.setDLPFMode(MPU6050_DLPF_BW_42);
}

void update_dmp(){
    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 = mpu.getIntStatus();
    
    //get current FIFO count;
    fifoCount = mpu.getFIFOCount();
    
    //check for overflow (only happens if your code sucks)
    if((mpuIntStatus & 0x10) || fifoCount == 1024){
        //reset so we can continue cleanly
        mpu.resetFIFO();
        DMP_DBG_MSG("\n\rFIFO overflow")
    } else if (mpuIntStatus & 0x02){
        int16_t x,y,z;
        //wait for correct available data length (should be very short)
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        //read a packet from FIFO
        mpu.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;
        
        mpu.dmpGetQuaternion(&q,fifoBuffer);
        imu_data.quat[0]=q.w;
        imu_data.quat[1]=q.x;
        imu_data.quat[2]=q.y;
        imu_data.quat[3]=q.z;
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr,&q,&gravity);
        imu_data.ypr[0] = ypr[0]*180/M_PI;//MARK - this saves the yaw data so everyone can access it
        imu_data.ypr[1] = ypr[1]*180/M_PI;
        imu_data.ypr[2] = ypr[2]*180/M_PI;
        
        mpu.getAcceleration(&x,&y,&z);
        imu_data.acc[0] = (x-acc_offset[0]) *9.81/16384;
        imu_data.acc[1] = (y-acc_offset[1]) *9.81/16384;
        imu_data.acc[2] = (z-acc_offset[2]) *9.81/16384;
        #ifdef PRINT_GYR
        DMP_DBG_MSG("\n\rypr\t %f\t %f\t %f",ypr[0]*180/PI,ypr[1]*180/PI,ypr[2]*180/PI)
        #endif
        
        mpu.dmpGetAccel(&aa, fifoBuffer);
        mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
        #ifdef PRINT_ACC
        DMP_DBG_MSG("\n\rareal\t%d\t%d\t%d",aaReal.x,aaReal.y,aaReal.z)
        #endif
                 //get rotations
        mpu.getMotion6(&x,&y,&z,&gx,&gy,&gz);
        float scale = 250/16384;
        imu_data.gyr[0] = gx*scale;
        imu_data.gyr[1] = gy*scale;
        imu_data.gyr[2] = gz*scale;
        
   }     
}
void update_dmp2(){
    if (!dmpReady2) return;
    while (!mpuInterrupt2 && fifoCount2 < packetSize2) {
        // 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
    mpuInterrupt2 = false;   //this resets the interrupt flag
    mpuIntStatus2 = mpu2.getIntStatus();
    
    //get current FIFO count;
    fifoCount2 = mpu2.getFIFOCount();
    
    //check for overflow (only happens if your code sucks)
    if((mpuIntStatus2 & 0x10) || fifoCount2 == 1024){
        //reset so we can continue cleanly
        mpu2.resetFIFO();
        DMP_DBG_MSG("\n\rFIFO overflow")
    } else if (mpuIntStatus2 & 0x02){
        int16_t x,y,z;
        //wait for correct available data length (should be very short)
        while (fifoCount2 < packetSize2) fifoCount2 = mpu2.getFIFOCount();

        //read a packet from FIFO
        mpu2.getFIFOBytes(fifoBuffer2, packetSize2);
        
        //track FIFO count here in the case there is > 1 packet available
        //(allows us to immediately read more w/o waiting for interrupt)
        fifoCount2 -= packetSize2;
        
        mpu2.dmpGetQuaternion(&q,fifoBuffer2);
        imu2_data.quat[0]=q.w;
        imu2_data.quat[1]=q.x;
        imu2_data.quat[2]=q.y;
        imu2_data.quat[3]=q.z;
        mpu2.dmpGetGravity(&gravity, &q);
        mpu2.dmpGetYawPitchRoll(ypr,&q,&gravity);
        imu2_data.ypr[0] = ypr[0]*180/M_PI;//MARK - this saves the yaw data so everyone can access it
        imu2_data.ypr[1] = ypr[1]*180/M_PI;
        imu2_data.ypr[2] = ypr[2]*180/M_PI;
        
        mpu2.getAcceleration(&x,&y,&z);
        imu2_data.acc[0] = (x-acc_offset[0]) *9.81/16384;
        imu2_data.acc[1] = (y-acc_offset[1]) *9.81/16384;
        imu2_data.acc[2] = (z-acc_offset[2]) *9.81/16384;
        #ifdef PRINT_GYR
        DMP_DBG_MSG("\n\rypr\t %f\t %f\t %f",ypr[0]*180/PI,ypr[1]*180/PI,ypr[2]*180/PI)
        #endif
        
        mpu2.dmpGetAccel(&aa, fifoBuffer2);
        mpu2.dmpGetLinearAccel(&aaReal, &aa, &gravity);
        #ifdef PRINT_ACC
        DMP_DBG_MSG("\n\rareal\t%d\t%d\t%d",aaReal.x,aaReal.y,aaReal.z)
        #endif
                 //get rotations
        mpu2.getMotion6(&x,&y,&z,&gx,&gy,&gz);
        float scale = 250/16384;
        imu2_data.gyr[0] = gx*scale;
        imu2_data.gyr[1] = gy*scale;
        imu2_data.gyr[2] = gz*scale;
        
   }     
}
void update_acc()
{
    int16_t x,y,z;
    mpu.getAcceleration(&x,&y,&z);
    imu_data.acc[0] = (x-acc_offset[0]) *9.81/16384;
    imu_data.acc[1] = (y-acc_offset[1]) *9.81/16384;
    imu_data.acc[2] = (z-acc_offset[2]) *9.81/16384;
}
void calibrate_1()    //call this to calibrate the accelerometer
{   //in the future, modify the OFFSET REGISTERS to make the DMP converge faster
    //right now, I am only calculating the offset for the accelerometers
    int sum[3] = {0,0,0};
    int i;
    int16_t x,y,z;
    
    int count=0;
    int t_now = t.read_ms();
    while((t.read_ms()-t_now) < 1000) //loop for one second
    {
        mpu.getAcceleration(&x,&y,&z);
        sum[0] += x;
        sum[1] += y;
        sum[2] += z;
        Thread::wait(5);
        count++;
    }
    for(i = 0; i<3; i++)
        acc_offset[i] = (sum[i]/count);
    //bt.printf("ACC_OFF:%d;%d;%d;",acc_offset[0],acc_offset[1],acc_offset[2]);//print the offset used by accelerometer
}