DMP.cpp

00001 //Copied MPU6050.h and MPU6050.cpp from another library (otherwise MPU gets stuck randomly)
00002 
00003 /*** THIS IS THE BEST VERSION OF DMP CODE
00004     This code demonstrates how to access the MPU6050 IMU and fix the 
00005     connection errors that occur, using a hardware hack.
00006     It requires two hardware fixes: A transistor (PTE5) and an extra GPIO pin (PTE23) connected
00007     to the SDA line.
00008     Error 1: The SDA line gets stuck on ground.
00009     Solution 1: Use the transistor to raise the SDA voltage off of ground. This resets the I2C bus. (line 71)
00010     Error 2: The SDA line gets stuck on 3.3V
00011     Solution 2: Use the GPIO line to pull the SDA voltage down a bit to reset the I2C bus. (line 96)
00012     (I am currently trying to find a software solution for the above hack)
00013     
00014     This code works for using the DMP. (See "DMP" for implementation that can do raw accel data)
00015     It appears all connection issues have been fixed.
00016     Next step: make code neater (in progress)
00017     
00018     LEARNED: Do not need the PTE23 line. Only fix necessary is the transistor (PTE5)
00019 ***/
00020 
00021 #include "mbed.h"
00022 #include "DMP.h"
00023 
00024 
00025 //******* MPU DECLARATION THINGS *****************//
00026 int dmp_test = 1;    //if this is 0, get raw MPU values
00027                  //if this is 1, get DMP values
00028 
00029 int acc_offset[3];  //this saves the offset for acceleration
00030  
00031 int16_t ax, ay, az;
00032 int16_t gx, gy, gz;
00033 
00034 //******* DMP things ****************//
00035 // MPU control/status vars
00036 bool dmpReady = false;  // set true if DMP init was succesfful
00037 uint8_t mpuIntStatus;   // holds interrupt status byte from MPU
00038 uint8_t devStatus;      // return status after each device operation (0 = success)
00039 uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
00040 uint16_t fifoCount;     // count of all bytes currently in FIFO
00041 uint8_t fifoBuffer[64]; // FIFO storage buffer
00042 bool dmpReady2 = false;  // set true if DMP init was succesfful
00043 uint8_t mpuIntStatus2;   // holds interrupt status byte from MPU
00044 uint8_t devStatus2;      // return status after each device operation (0 = success)
00045 uint16_t packetSize2;    // expected DMP packet size (default is 42 bytes)
00046 uint16_t fifoCount2;     // count of all bytes currently in FIFO
00047 uint8_t fifoBuffer2[64]; // FIFO storage buffer
00048 
00049     
00050     
00051 uint8_t MPU_CONNECTION;
00052         
00053 // orientation/motion vars
00054 Quaternion q;           // [w,x,y,z]    quaternion container
00055 VectorInt16 aa;         // [x,y,z]      accel sensor measurements
00056 VectorInt16 aaReal;     // [x,y,z]      gravity-free accel sensor measurements
00057 VectorInt16 aaWorld;    // [x,y,z]      world-frame accel sensor measurements
00058 VectorFloat gravity;    // [x,y,z]      gravity vector
00059 float euler[3];         // [psi,theta,phi]  Euler angle container
00060 float ypr[3];           // [yaw,pitch,roll] yaw/pitch/roll container [rad]. Multiply by 180, divide by PI for [deg]
00061 InterruptIn checkpin(PTD7); //interrupt
00062 InterruptIn checkpin2(PTD5); //PTD5
00063 //*** Interrupt Detection Routine ***//
00064 volatile bool mpuInterrupt = false; //indicates whether interrupt pin has gone high
00065 void dmpDataReady(){
00066     mpuInterrupt = true;
00067 }
00068 volatile bool mpuInterrupt2 = false; //indicates whether interrupt pin has gone high
00069 void dmpDataReady2(){
00070     mpuInterrupt2 = true;
00071 }
00072         
00073 //****** END MPU DECLARATION ********************//
00074 
00075 
00076 int test_dmp();    //this wraps up the code req'd to start the DMP
00077 void start_dmp(MPU6050);    //this will get the DMP ready
00078 void update_dmp();   //call this frequently
00079 
00080 //*********************FUNCTIONS***************************************************************//
00081 int test_dmp(){ //returns 1 if ok (right now, it doesn't return at all if there is a problem    
00082     imuSwitch = 1;
00083     LED_OFF;
00084     Thread::wait(100);
00085     
00086     while(mpu.testConnection()==0)
00087     {
00088         imuSwitch = 0;
00089         Thread::wait(100);
00090         imuSwitch = 1;
00091         Thread::wait(100);
00092         bt.lock();
00093         bt.printf("Stuck on IMU1");
00094         bt.unlock();
00095     }
00096     imuSwitch = 1;
00097     LED_ON  //turn ON LED
00098     return 1;
00099 }
00100 int test_dmp2(){ //returns 1 if ok (right now, it doesn't return at all if there is a problem    
00101     imu2Switch = 1;
00102     LED_OFF;
00103     Thread::wait(100);
00104     
00105     while(mpu2.testConnection()==0)
00106     {
00107         imu2Switch = 0;
00108         Thread::wait(100);
00109         imu2Switch = 1;
00110         Thread::wait(100);
00111         bt.lock();
00112         bt.printf("Stuck on IMU2");
00113         bt.unlock();
00114     }
00115     imu2Switch = 1;
00116     LED_ON  //turn ON LED
00117     return 1;
00118 }
00119 
00120 void start_dmp(MPU6050 mpu1){    //this will get the DMP ready
00121     //initialize device
00122     mpu1.reset();
00123     DMP_DBG_MSG("\n\rInitializing I2C devices...")
00124     devStatus = mpu1.dmpInitialize();
00125     //insert gyro offsets here// Write a calibration algorithm in the future
00126     mpu1.setXGyroOffset(0); //800/25=32 //-31 is the largest offset available
00127     mpu1.setYGyroOffset(0);
00128     mpu1.setZGyroOffset(0);
00129     mpu1.setXAccelOffset(0);     //the accel offsets don't do anything
00130     mpu1.setYAccelOffset(0);
00131     mpu1.setZAccelOffset(0);
00132     
00133     mpu1.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
00134     
00135     if(devStatus == 0){     //this means initialize was successful
00136         //turn on DMP
00137         DMP_DBG_MSG("\n\rEnabling DMP")
00138         mpu1.setDMPEnabled(true);
00139         
00140         // enable  interrupt detection
00141         DMP_DBG_MSG("Enabling interrupt detection (Arduino external interrupt 0)...\r\n")
00142         checkpin.rise(&dmpDataReady);
00143     
00144         mpuIntStatus = mpu1.getIntStatus();
00145         
00146         //set the DMP ready flag so main loop knows when it is ok to use
00147         DMP_DBG_MSG("DMP ready! Waiting for first interrupt")
00148         dmpReady = true;
00149         
00150         //get expected DMP packet size
00151         packetSize = mpu1.dmpGetFIFOPacketSize();
00152     }else{
00153         DMP_DBG_MSG("\n\rDMP Initialization failed")
00154         DMP_DBG_MSG("\t%d",devStatus)
00155         Thread::wait(1000);
00156     }
00157     acc_offset[0]=0;
00158     acc_offset[1]=0;
00159     acc_offset[2]=0;
00160     mpu.setDLPFMode(MPU6050_DLPF_BW_42);
00161 }
00162 void start_dmp2(MPU6051 mpu1){    //this will get the DMP ready
00163     //initialize device
00164     mpu2.reset();
00165     DMP_DBG_MSG("\n\rInitializing I2C devices...")
00166     devStatus2 = mpu2.dmpInitialize();
00167     //insert gyro offsets here// Write a calibration algorithm in the future
00168     mpu2.setXGyroOffset(0); //800/25=32 //-31 is the largest offset available
00169     mpu2.setYGyroOffset(0);
00170     mpu2.setZGyroOffset(0);
00171     mpu2.setXAccelOffset(0);     //the accel offsets don't do anything
00172     mpu2.setYAccelOffset(0);
00173     mpu2.setZAccelOffset(0);
00174     
00175     mpu2.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
00176     
00177     if(devStatus2 == 0){     //this means initialize was successful
00178         //turn on DMP
00179         DMP_DBG_MSG("\n\rEnabling DMP")
00180         mpu2.setDMPEnabled(true);
00181         
00182         // enable  interrupt detection
00183         DMP_DBG_MSG("Enabling interrupt detection (Arduino external interrupt 0)...\r\n")
00184         checkpin2.rise(&dmpDataReady2);
00185     
00186         mpuIntStatus2 = mpu2.getIntStatus();
00187         
00188         //set the DMP ready flag so main loop knows when it is ok to use
00189         DMP_DBG_MSG("DMP ready! Waiting for first interrupt")
00190         dmpReady2 = true;
00191         
00192         //get expected DMP packet size
00193         packetSize2 = mpu2.dmpGetFIFOPacketSize();
00194     }else{
00195         DMP_DBG_MSG("\n\rDMP Initialization failed")
00196         DMP_DBG_MSG("\t%d",devStatus2)
00197         Thread::wait(1000);
00198     }
00199     acc_offset[0]=0;
00200     acc_offset[1]=0;
00201     acc_offset[2]=0;
00202     mpu2.setDLPFMode(MPU6050_DLPF_BW_42);
00203 }
00204 
00205 void update_dmp(){
00206     if (!dmpReady) return;
00207     while (!mpuInterrupt && fifoCount < packetSize) {
00208         // other program behavior stuff here
00209         // if you are really paranoid you can frequently test in between other
00210         // stuff to see if mpuInterrupt is true, and if so, "break;" from the
00211         // while() loop to immediately process the MPU data
00212         // .
00213     }
00214     // reset interrupt flag and get INT_STATUS byte
00215     mpuInterrupt = false;   //this resets the interrupt flag
00216     mpuIntStatus = mpu.getIntStatus();
00217     
00218     //get current FIFO count;
00219     fifoCount = mpu.getFIFOCount();
00220     
00221     //check for overflow (only happens if your code sucks)
00222     if((mpuIntStatus & 0x10) || fifoCount == 1024){
00223         //reset so we can continue cleanly
00224         mpu.resetFIFO();
00225         DMP_DBG_MSG("\n\rFIFO overflow")
00226     } else if (mpuIntStatus & 0x02){
00227         int16_t x,y,z;
00228         //wait for correct available data length (should be very short)
00229         while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
00230 
00231         //read a packet from FIFO
00232         mpu.getFIFOBytes(fifoBuffer, packetSize);
00233         
00234         //track FIFO count here in the case there is > 1 packet available
00235         //(allows us to immediately read more w/o waiting for interrupt)
00236         fifoCount -= packetSize;
00237         
00238         mpu.dmpGetQuaternion(&q,fifoBuffer);
00239         imu_data.quat[0]=q.w;
00240         imu_data.quat[1]=q.x;
00241         imu_data.quat[2]=q.y;
00242         imu_data.quat[3]=q.z;
00243         mpu.dmpGetGravity(&gravity, &q);
00244         mpu.dmpGetYawPitchRoll(ypr,&q,&gravity);
00245         imu_data.ypr[0] = ypr[0]*180/M_PI;//MARK - this saves the yaw data so everyone can access it
00246         imu_data.ypr[1] = ypr[1]*180/M_PI;
00247         imu_data.ypr[2] = ypr[2]*180/M_PI;
00248         
00249         mpu.getAcceleration(&x,&y,&z);
00250         imu_data.acc[0] = (x-acc_offset[0]) *9.81/16384;
00251         imu_data.acc[1] = (y-acc_offset[1]) *9.81/16384;
00252         imu_data.acc[2] = (z-acc_offset[2]) *9.81/16384;
00253         #ifdef PRINT_GYR
00254         DMP_DBG_MSG("\n\rypr\t %f\t %f\t %f",ypr[0]*180/PI,ypr[1]*180/PI,ypr[2]*180/PI)
00255         #endif
00256         
00257         mpu.dmpGetAccel(&aa, fifoBuffer);
00258         mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
00259         #ifdef PRINT_ACC
00260         DMP_DBG_MSG("\n\rareal\t%d\t%d\t%d",aaReal.x,aaReal.y,aaReal.z)
00261         #endif
00262                  //get rotations
00263         mpu.getMotion6(&x,&y,&z,&gx,&gy,&gz);
00264         float scale = 250/16384;
00265         imu_data.gyr[0] = gx*scale;
00266         imu_data.gyr[1] = gy*scale;
00267         imu_data.gyr[2] = gz*scale;
00268         
00269    }     
00270 }
00271 void update_dmp2(){
00272     if (!dmpReady2) return;
00273     while (!mpuInterrupt2 && fifoCount2 < packetSize2) {
00274         // other program behavior stuff here
00275         // if you are really paranoid you can frequently test in between other
00276         // stuff to see if mpuInterrupt is true, and if so, "break;" from the
00277         // while() loop to immediately process the MPU data
00278         // .
00279     }
00280     // reset interrupt flag and get INT_STATUS byte
00281     mpuInterrupt2 = false;   //this resets the interrupt flag
00282     mpuIntStatus2 = mpu2.getIntStatus();
00283     
00284     //get current FIFO count;
00285     fifoCount2 = mpu2.getFIFOCount();
00286     
00287     //check for overflow (only happens if your code sucks)
00288     if((mpuIntStatus2 & 0x10) || fifoCount2 == 1024){
00289         //reset so we can continue cleanly
00290         mpu2.resetFIFO();
00291         DMP_DBG_MSG("\n\rFIFO overflow")
00292     } else if (mpuIntStatus2 & 0x02){
00293         int16_t x,y,z;
00294         //wait for correct available data length (should be very short)
00295         while (fifoCount2 < packetSize2) fifoCount2 = mpu2.getFIFOCount();
00296 
00297         //read a packet from FIFO
00298         mpu2.getFIFOBytes(fifoBuffer2, packetSize2);
00299         
00300         //track FIFO count here in the case there is > 1 packet available
00301         //(allows us to immediately read more w/o waiting for interrupt)
00302         fifoCount2 -= packetSize2;
00303         
00304         mpu2.dmpGetQuaternion(&q,fifoBuffer2);
00305         imu2_data.quat[0]=q.w;
00306         imu2_data.quat[1]=q.x;
00307         imu2_data.quat[2]=q.y;
00308         imu2_data.quat[3]=q.z;
00309         mpu2.dmpGetGravity(&gravity, &q);
00310         mpu2.dmpGetYawPitchRoll(ypr,&q,&gravity);
00311         imu2_data.ypr[0] = ypr[0]*180/M_PI;//MARK - this saves the yaw data so everyone can access it
00312         imu2_data.ypr[1] = ypr[1]*180/M_PI;
00313         imu2_data.ypr[2] = ypr[2]*180/M_PI;
00314         
00315         mpu2.getAcceleration(&x,&y,&z);
00316         imu2_data.acc[0] = (x-acc_offset[0]) *9.81/16384;
00317         imu2_data.acc[1] = (y-acc_offset[1]) *9.81/16384;
00318         imu2_data.acc[2] = (z-acc_offset[2]) *9.81/16384;
00319         #ifdef PRINT_GYR
00320         DMP_DBG_MSG("\n\rypr\t %f\t %f\t %f",ypr[0]*180/PI,ypr[1]*180/PI,ypr[2]*180/PI)
00321         #endif
00322         
00323         mpu2.dmpGetAccel(&aa, fifoBuffer2);
00324         mpu2.dmpGetLinearAccel(&aaReal, &aa, &gravity);
00325         #ifdef PRINT_ACC
00326         DMP_DBG_MSG("\n\rareal\t%d\t%d\t%d",aaReal.x,aaReal.y,aaReal.z)
00327         #endif
00328                  //get rotations
00329         mpu2.getMotion6(&x,&y,&z,&gx,&gy,&gz);
00330         float scale = 250/16384;
00331         imu2_data.gyr[0] = gx*scale;
00332         imu2_data.gyr[1] = gy*scale;
00333         imu2_data.gyr[2] = gz*scale;
00334         
00335    }     
00336 }
00337 void update_acc()
00338 {
00339     int16_t x,y,z;
00340     mpu.getAcceleration(&x,&y,&z);
00341     imu_data.acc[0] = (x-acc_offset[0]) *9.81/16384;
00342     imu_data.acc[1] = (y-acc_offset[1]) *9.81/16384;
00343     imu_data.acc[2] = (z-acc_offset[2]) *9.81/16384;
00344 }
00345 void calibrate_1()    //call this to calibrate the accelerometer
00346 {   //in the future, modify the OFFSET REGISTERS to make the DMP converge faster
00347     //right now, I am only calculating the offset for the accelerometers
00348     int sum[3] = {0,0,0};
00349     int i;
00350     int16_t x,y,z;
00351     
00352     int count=0;
00353     int t_now = t.read_ms();
00354     while((t.read_ms()-t_now) < 1000) //loop for one second
00355     {
00356         mpu.getAcceleration(&x,&y,&z);
00357         sum[0] += x;
00358         sum[1] += y;
00359         sum[2] += z;
00360         Thread::wait(5);
00361         count++;
00362     }
00363     for(i = 0; i<3; i++)
00364         acc_offset[i] = (sum[i]/count);
00365     //bt.printf("ACC_OFF:%d;%d;%d;",acc_offset[0],acc_offset[1],acc_offset[2]);//print the offset used by accelerometer
00366 }