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 Quaternion q2;           // [w,x,y,z]    quaternion container
00056 VectorInt16 aa;         // [x,y,z]      accel sensor measurements
00057 VectorInt16 aaReal;     // [x,y,z]      gravity-free accel sensor measurements
00058 VectorInt16 aaWorld;    // [x,y,z]      world-frame accel sensor measurements
00059 VectorFloat gravity;    // [x,y,z]      gravity vector
00060 VectorFloat gravity2;    // [x,y,z]      gravity vector
00061 
00062 float euler[3];         // [psi,theta,phi]  Euler angle container
00063 float ypr[3];           // [yaw,pitch,roll] yaw/pitch/roll container [rad]. Multiply by 180, divide by PI for [deg]
00064 float ypr2[3]; 
00065 InterruptIn checkpin(PTD7); //interrupt
00066 InterruptIn checkpin2(PTD5); //PTD5
00067 //*** Interrupt Detection Routine ***//
00068 volatile bool mpuInterrupt = false; //indicates whether interrupt pin has gone high
00069 void dmpDataReady(){
00070     mpuInterrupt = true;
00071 }
00072 volatile bool mpuInterrupt2 = false; //indicates whether interrupt pin has gone high
00073 void dmpDataReady2(){
00074     mpuInterrupt2 = true;
00075 }
00076         
00077 //****** END MPU DECLARATION ********************//
00078 
00079 
00080 int test_dmp();    //this wraps up the code req'd to start the DMP
00081 void start_dmp(MPU6050);    //this will get the DMP ready
00082 void update_dmp();   //call this frequently
00083 
00084 //*********************FUNCTIONS***************************************************************//
00085 int test_dmp(){ //returns 1 if ok (right now, it doesn't return at all if there is a problem    
00086     imuSwitch = 1;
00087     LED_OFF;
00088     Thread::wait(100);
00089     
00090     while(mpu.testConnection()==0)
00091     {
00092         imuSwitch = 0;
00093         Thread::wait(100);
00094         imuSwitch = 1;
00095         Thread::wait(100);
00096         bt.lock();
00097         bt.printf("Stuck on IMU1");
00098         bt.unlock();
00099     }
00100     imuSwitch = 1;
00101     LED_ON  //turn ON LED
00102     return 1;
00103 }
00104 int test_dmp2(){ //returns 1 if ok (right now, it doesn't return at all if there is a problem    
00105     imu2Switch = 1;
00106     LED_OFF;
00107     Thread::wait(100);
00108     
00109     while(mpu2.testConnection()==0)
00110     {
00111         imu2Switch = 0;
00112         Thread::wait(100);
00113         imu2Switch = 1;
00114         Thread::wait(100);
00115         bt.lock();
00116         bt.printf("Stuck on IMU2");
00117         bt.unlock();
00118     }
00119     imu2Switch = 1;
00120     LED_ON  //turn ON LED
00121     return 1;
00122 }
00123 
00124 void start_dmp(MPU6050 mpu1){    //this will get the DMP ready
00125     //initialize device
00126     mpu1.reset();
00127     DMP_DBG_MSG("\n\rInitializing I2C devices...")
00128     devStatus = mpu1.dmpInitialize();
00129     //insert gyro offsets here// Write a calibration algorithm in the future
00130     mpu1.setXGyroOffset(0); //800/25=32 //-31 is the largest offset available
00131     mpu1.setYGyroOffset(0);
00132     mpu1.setZGyroOffset(0);
00133     mpu1.setXAccelOffset(0);     //the accel offsets don't do anything
00134     mpu1.setYAccelOffset(0);
00135     mpu1.setZAccelOffset(0);
00136     
00137     mpu1.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
00138     
00139     if(devStatus == 0){     //this means initialize was successful
00140         //turn on DMP
00141         DMP_DBG_MSG("\n\rEnabling DMP")
00142         mpu1.setDMPEnabled(true);
00143         
00144         // enable  interrupt detection
00145         DMP_DBG_MSG("Enabling interrupt detection (Arduino external interrupt 0)...\r\n")
00146         checkpin.rise(&dmpDataReady);
00147     
00148         mpuIntStatus = mpu1.getIntStatus();
00149         
00150         //set the DMP ready flag so main loop knows when it is ok to use
00151         DMP_DBG_MSG("DMP ready! Waiting for first interrupt")
00152         dmpReady = true;
00153         
00154         //get expected DMP packet size
00155         packetSize = mpu1.dmpGetFIFOPacketSize();
00156     }else{
00157         DMP_DBG_MSG("\n\rDMP Initialization failed")
00158         DMP_DBG_MSG("\t%d",devStatus)
00159         Thread::wait(1000);
00160     }
00161     acc_offset[0]=0;
00162     acc_offset[1]=0;
00163     acc_offset[2]=0;
00164     mpu.setDLPFMode(MPU6050_DLPF_BW_42);
00165 }
00166 void start_dmp2(MPU6051 mpu1){    //this will get the DMP ready
00167     //initialize device
00168     mpu2.reset();
00169     DMP_DBG_MSG("\n\rInitializing I2C devices...")
00170     devStatus2 = mpu2.dmpInitialize();
00171     //insert gyro offsets here// Write a calibration algorithm in the future
00172     mpu2.setXGyroOffset(0); //800/25=32 //-31 is the largest offset available
00173     mpu2.setYGyroOffset(0);
00174     mpu2.setZGyroOffset(0);
00175     mpu2.setXAccelOffset(0);     //the accel offsets don't do anything
00176     mpu2.setYAccelOffset(0);
00177     mpu2.setZAccelOffset(0);
00178     
00179     mpu2.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
00180     
00181     if(devStatus2 == 0){     //this means initialize was successful
00182         //turn on DMP
00183         DMP_DBG_MSG("\n\rEnabling DMP")
00184         mpu2.setDMPEnabled(true);
00185         
00186         // enable  interrupt detection
00187         DMP_DBG_MSG("Enabling interrupt detection (Arduino external interrupt 0)...\r\n")
00188         checkpin2.rise(&dmpDataReady2);
00189     
00190         mpuIntStatus2 = mpu2.getIntStatus();
00191         
00192         //set the DMP ready flag so main loop knows when it is ok to use
00193         DMP_DBG_MSG("DMP ready! Waiting for first interrupt")
00194         dmpReady2 = true;
00195         
00196         //get expected DMP packet size
00197         packetSize2 = mpu2.dmpGetFIFOPacketSize();
00198     }else{
00199         DMP_DBG_MSG("\n\rDMP Initialization failed")
00200         DMP_DBG_MSG("\t%d",devStatus2)
00201         Thread::wait(1000);
00202     }
00203     acc_offset[0]=0;
00204     acc_offset[1]=0;
00205     acc_offset[2]=0;
00206     mpu2.setDLPFMode(MPU6050_DLPF_BW_42);
00207 }
00208 
00209 void update_dmp(){
00210     if (!dmpReady) return;
00211     while (!mpuInterrupt && fifoCount < packetSize) {
00212         // other program behavior stuff here
00213         // if you are really paranoid you can frequently test in between other
00214         // stuff to see if mpuInterrupt is true, and if so, "break;" from the
00215         // while() loop to immediately process the MPU data
00216         // .
00217     }
00218     // reset interrupt flag and get INT_STATUS byte
00219     mpuInterrupt = false;   //this resets the interrupt flag
00220     mpuIntStatus = mpu.getIntStatus();
00221     
00222     //get current FIFO count;
00223     fifoCount = mpu.getFIFOCount();
00224     
00225     //check for overflow (only happens if your code sucks)
00226     if((mpuIntStatus & 0x10) || fifoCount == 1024){
00227         //reset so we can continue cleanly
00228         mpu.resetFIFO();
00229         DMP_DBG_MSG("\n\rFIFO overflow")
00230     } else if (mpuIntStatus & 0x02){
00231         int16_t x,y,z;
00232         //wait for correct available data length (should be very short)
00233         while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
00234 
00235         //read a packet from FIFO
00236         mpu.getFIFOBytes(fifoBuffer, packetSize);
00237         
00238         //track FIFO count here in the case there is > 1 packet available
00239         //(allows us to immediately read more w/o waiting for interrupt)
00240         fifoCount -= packetSize;
00241         
00242         mpu.dmpGetQuaternion(&q,fifoBuffer);
00243         imu_data.quat[0]=q.w;
00244         imu_data.quat[1]=q.x;
00245         imu_data.quat[2]=q.y;
00246         imu_data.quat[3]=q.z;
00247         mpu.dmpGetGravity(&gravity, &q);
00248         mpu.dmpGetYawPitchRoll(ypr,&q,&gravity);
00249         imu_data.ypr[0] = ypr[0]*180/M_PI;//MARK - this saves the yaw data so everyone can access it
00250         imu_data.ypr[1] = ypr[1]*180/M_PI;
00251         imu_data.ypr[2] = ypr[2]*180/M_PI;
00252         
00253         mpu.getAcceleration(&x,&y,&z);
00254         imu_data.acc[0] = (x-acc_offset[0]) *9.81/16384;
00255         imu_data.acc[1] = (y-acc_offset[1]) *9.81/16384;
00256         imu_data.acc[2] = (z-acc_offset[2]) *9.81/16384;
00257         #ifdef PRINT_GYR
00258         DMP_DBG_MSG("\n\rypr\t %f\t %f\t %f",ypr[0]*180/PI,ypr[1]*180/PI,ypr[2]*180/PI)
00259         #endif
00260         
00261         mpu.dmpGetAccel(&aa, fifoBuffer);
00262         mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
00263         #ifdef PRINT_ACC
00264         DMP_DBG_MSG("\n\rareal\t%d\t%d\t%d",aaReal.x,aaReal.y,aaReal.z)
00265         #endif
00266                  //get rotations
00267         mpu.getMotion6(&x,&y,&z,&gx,&gy,&gz);
00268         float scale = 250/16384;
00269         imu_data.gyr[0] = gx*scale;
00270         imu_data.gyr[1] = gy*scale;
00271         imu_data.gyr[2] = gz*scale;
00272         
00273    }     
00274 }
00275 void update_dmp2(){
00276     if (!dmpReady2) return;
00277     while (!mpuInterrupt2 && fifoCount2 < packetSize2) {
00278         // other program behavior stuff here
00279         // if you are really paranoid you can frequently test in between other
00280         // stuff to see if mpuInterrupt is true, and if so, "break;" from the
00281         // while() loop to immediately process the MPU data
00282         // .
00283     }
00284     // reset interrupt flag and get INT_STATUS byte
00285     mpuInterrupt2 = false;   //this resets the interrupt flag
00286     mpuIntStatus2 = mpu2.getIntStatus();
00287     
00288     //get current FIFO count;
00289     fifoCount2 = mpu2.getFIFOCount();
00290     
00291     //check for overflow (only happens if your code sucks)
00292     if((mpuIntStatus2 & 0x10) || fifoCount2 == 1024){
00293         //reset so we can continue cleanly
00294         mpu2.resetFIFO();
00295         DMP_DBG_MSG("\n\rFIFO overflow")
00296     } else if (mpuIntStatus2 & 0x02){
00297         int16_t x,y,z;
00298         //wait for correct available data length (should be very short)
00299         while (fifoCount2 < packetSize2) fifoCount2 = mpu2.getFIFOCount();
00300 
00301         //read a packet from FIFO
00302         mpu2.getFIFOBytes(fifoBuffer2, packetSize2);
00303         
00304         //track FIFO count here in the case there is > 1 packet available
00305         //(allows us to immediately read more w/o waiting for interrupt)
00306         fifoCount2 -= packetSize2;
00307         
00308         mpu2.dmpGetQuaternion(&q2,fifoBuffer2);
00309         imu2_data.quat[0]=q2.w;
00310         imu2_data.quat[1]=q2.x;
00311         imu2_data.quat[2]=q2.y;
00312         imu2_data.quat[3]=q2.z;
00313         mpu2.dmpGetGravity(&gravity2, &q2);
00314         mpu2.dmpGetYawPitchRoll(ypr2,&q2,&gravity2);
00315         imu2_data.ypr[0] = ypr2[0]*180/M_PI;//MARK - this saves the yaw data so everyone can access it
00316         imu2_data.ypr[1] = ypr2[1]*180/M_PI;
00317         imu2_data.ypr[2] = ypr2[2]*180/M_PI;
00318         
00319         mpu2.getAcceleration(&x,&y,&z);
00320         imu2_data.acc[0] = (x-acc_offset[0]) *9.81/16384;
00321         imu2_data.acc[1] = (y-acc_offset[1]) *9.81/16384;
00322         imu2_data.acc[2] = (z-acc_offset[2]) *9.81/16384;
00323         #ifdef PRINT_GYR
00324         DMP_DBG_MSG("\n\rypr\t %f\t %f\t %f",ypr2[0]*180/PI,ypr2[1]*180/PI,ypr2[2]*180/PI)
00325         #endif
00326         
00327         mpu2.dmpGetAccel(&aa, fifoBuffer2);
00328         mpu2.dmpGetLinearAccel(&aaReal, &aa, &gravity2);
00329         #ifdef PRINT_ACC
00330         DMP_DBG_MSG("\n\rareal\t%d\t%d\t%d",aaReal.x,aaReal.y,aaReal.z)
00331         #endif
00332                  //get rotations
00333         mpu2.getMotion6(&x,&y,&z,&gx,&gy,&gz);
00334         float scale = 250/16384;
00335         imu2_data.gyr[0] = gx*scale;
00336         imu2_data.gyr[1] = gy*scale;
00337         imu2_data.gyr[2] = gz*scale;
00338         
00339    }     
00340 }
00341 void update_acc()
00342 {
00343     int16_t x,y,z;
00344     mpu.getAcceleration(&x,&y,&z);
00345     imu_data.acc[0] = (x-acc_offset[0]) *9.81/16384;
00346     imu_data.acc[1] = (y-acc_offset[1]) *9.81/16384;
00347     imu_data.acc[2] = (z-acc_offset[2]) *9.81/16384;
00348 }
00349 void calibrate_1()    //call this to calibrate the accelerometer
00350 {   //in the future, modify the OFFSET REGISTERS to make the DMP converge faster
00351     //right now, I am only calculating the offset for the accelerometers
00352     int sum[3] = {0,0,0};
00353     int i;
00354     int16_t x,y,z;
00355     
00356     int count=0;
00357     int t_now = t.read_ms();
00358     while((t.read_ms()-t_now) < 1000) //loop for one second
00359     {
00360         mpu.getAcceleration(&x,&y,&z);
00361         sum[0] += x;
00362         sum[1] += y;
00363         sum[2] += z;
00364         Thread::wait(5);
00365         count++;
00366     }
00367     for(i = 0; i<3; i++)
00368         acc_offset[i] = (sum[i]/count);
00369     //bt.printf("ACC_OFF:%d;%d;%d;",acc_offset[0],acc_offset[1],acc_offset[2]);//print the offset used by accelerometer
00370 }