Diff: DMP.cpp

Revision:

0:dceb852b19f3

diff -r 000000000000 -r dceb852b19f3 DMP.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DMP.cpp	Tue May 27 09:23:23 2014 +0000
@@ -0,0 +1,205 @@
+//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
+   }     
+}
\ No newline at end of file