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