Mark Vandermeulen / Mbed 2 deprecated DMP_tidy_working

asdfsaf

Dependencies:   MPU6050-DMP-Ian mbed-rtos mbed

DMP.cpp@0:dceb852b19f3, 2014-05-27 (annotated)

Committer:
majik
Date:
Tue May 27 09:23:23 2014 +0000
Revision:
0:dceb852b19f3
dfsasf

Who changed what in which revision?

UserRevisionLine numberNew contents of line
majik 0:dceb852b19f3 1 //Copied MPU6050.h and MPU6050.cpp from another library (otherwise MPU gets stuck randomly)
majik 0:dceb852b19f3 2
majik 0:dceb852b19f3 3 /*** THIS IS THE BEST VERSION OF DMP CODE
majik 0:dceb852b19f3 4 This code demonstrates how to access the MPU6050 IMU and fix the
majik 0:dceb852b19f3 5 connection errors that occur, using a hardware hack.
majik 0:dceb852b19f3 6 It requires two hardware fixes: A transistor (PTE5) and an extra GPIO pin (PTE23) connected
majik 0:dceb852b19f3 7 to the SDA line.
majik 0:dceb852b19f3 8 Error 1: The SDA line gets stuck on ground.
majik 0:dceb852b19f3 9 Solution 1: Use the transistor to raise the SDA voltage off of ground. This resets the I2C bus. (line 71)
majik 0:dceb852b19f3 10 Error 2: The SDA line gets stuck on 3.3V
majik 0:dceb852b19f3 11 Solution 2: Use the GPIO line to pull the SDA voltage down a bit to reset the I2C bus. (line 96)
majik 0:dceb852b19f3 12 (I am currently trying to find a software solution for the above hack)
majik 0:dceb852b19f3 13
majik 0:dceb852b19f3 14 This code works for using the DMP. (See "DMP" for implementation that can do raw accel data)
majik 0:dceb852b19f3 15 It appears all connection issues have been fixed.
majik 0:dceb852b19f3 16 Next step: make code neater (in progress)
majik 0:dceb852b19f3 17
majik 0:dceb852b19f3 18 LEARNED: Do not need the PTE23 line. Only fix necessary is the transistor (PTE5)
majik 0:dceb852b19f3 19 ***/
majik 0:dceb852b19f3 20
majik 0:dceb852b19f3 21 #include "mbed.h"
majik 0:dceb852b19f3 22 #include "I2Cdev.h"
majik 0:dceb852b19f3 23 #include "MPU6050_6Axis_MotionApps20.h"
majik 0:dceb852b19f3 24 #include "robot.h"
majik 0:dceb852b19f3 25
majik 0:dceb852b19f3 26
majik 0:dceb852b19f3 27 //******* MPU DECLARATION THINGS *****************//
majik 0:dceb852b19f3 28 int dmp_test =1; //if this is 0, get raw MPU values
majik 0:dceb852b19f3 29 //if this is 1, get DMP values
majik 0:dceb852b19f3 30
majik 0:dceb852b19f3 31 int16_t ax, ay, az;
majik 0:dceb852b19f3 32 int16_t gx, gy, gz;
majik 0:dceb852b19f3 33
majik 0:dceb852b19f3 34 //******* DMP things ****************//
majik 0:dceb852b19f3 35 // MPU control/status vars
majik 0:dceb852b19f3 36 bool dmpReady = false; // set true if DMP init was succesfful
majik 0:dceb852b19f3 37 uint8_t mpuIntStatus; // holds interrupt status byte from MPU
majik 0:dceb852b19f3 38 uint8_t devStatus; // return status after each device operation (0 = success)
majik 0:dceb852b19f3 39 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
majik 0:dceb852b19f3 40 uint16_t fifoCount; // count of all bytes currently in FIFO
majik 0:dceb852b19f3 41 uint8_t fifoBuffer[64]; // FIFO storage buffer
majik 0:dceb852b19f3 42
majik 0:dceb852b19f3 43
majik 0:dceb852b19f3 44 uint8_t MPU_CONNECTION;
majik 0:dceb852b19f3 45
majik 0:dceb852b19f3 46 // orientation/motion vars
majik 0:dceb852b19f3 47 Quaternion q; // [w,x,y,z] quaternion container
majik 0:dceb852b19f3 48 VectorInt16 aa; // [x,y,z] accel sensor measurements
majik 0:dceb852b19f3 49 VectorInt16 aaReal; // [x,y,z] gravity-free accel sensor measurements
majik 0:dceb852b19f3 50 VectorInt16 aaWorld; // [x,y,z] world-frame accel sensor measurements
majik 0:dceb852b19f3 51 VectorFloat gravity; // [x,y,z] gravity vector
majik 0:dceb852b19f3 52 float euler[3]; // [psi,theta,phi] Euler angle container
majik 0:dceb852b19f3 53 float ypr[3]; // [yaw,pitch,roll] yaw/pitch/roll container [rad]. Multiply by 180, divide by PI for [deg]
majik 0:dceb852b19f3 54 InterruptIn checkpin(PTD7); //interrupt
majik 0:dceb852b19f3 55 //*** Interrupt Detection Routine ***//
majik 0:dceb852b19f3 56 volatile bool mpuInterrupt = false; //indicates whether interrupt pin has gone high
majik 0:dceb852b19f3 57 void dmpDataReady(){
majik 0:dceb852b19f3 58 mpuInterrupt = true;
majik 0:dceb852b19f3 59 }
majik 0:dceb852b19f3 60
majik 0:dceb852b19f3 61 //****** END MPU DECLARATION ********************//
majik 0:dceb852b19f3 62
majik 0:dceb852b19f3 63
majik 0:dceb852b19f3 64 int test_dmp(); //this wraps up the code req'd to start the DMP
majik 0:dceb852b19f3 65 void start_dmp(MPU6050 mpu); //this will get the DMP ready
majik 0:dceb852b19f3 66 void update_dmp(MPU6050 mpu); //call this frequently
majik 0:dceb852b19f3 67
majik 0:dceb852b19f3 68 //*********************FUNCTIONS***************************************************************//
majik 0:dceb852b19f3 69 int test_dmp(){ //returns 1 if ok (right now, it doesn't return at all if there is a problem
majik 0:dceb852b19f3 70 /// This function tests the I2C port for the MPU6050 and resets it if it is not working
majik 0:dceb852b19f3 71 //NOTE: when the I2C connection is not successful, calling initialize() will cause the board to freeze
majik 0:dceb852b19f3 72 DigitalOut reset_pin(PTE5); //Transistor to pull up SDA line
majik 0:dceb852b19f3 73 reset_pin = 0; //start the transistor in OFF mode
majik 0:dceb852b19f3 74 //int count = 0;
majik 0:dceb852b19f3 75 DigitalIn sample_1(PTE0); //(this pin will be used as the SDA pin when an MPU6050 object is declared
majik 0:dceb852b19f3 76 int temp = sample_1; //Sample what is happening on the SDA line to guess what the solution is.
majik 0:dceb852b19f3 77 myled = 1; //turn OFF LED. This will turn on again when the MPU is connected.
majik 0:dceb852b19f3 78 //bt.baud(baudRate);
majik 0:dceb852b19f3 79 DigitalOut reset_1(PTE0);//change the SDA pin to output.
majik 0:dceb852b19f3 80 reset_1 = 0;
majik 0:dceb852b19f3 81 if(temp == 1){ //if SDA line starts high, unstick by pulling it low
majik 0:dceb852b19f3 82 bt.printf("\n\rSetting up I2C connection...");
majik 0:dceb852b19f3 83 reset_1 = 0; //SDA pin = low
majik 0:dceb852b19f3 84 wait_ms(100);
majik 0:dceb852b19f3 85 MPU6050 *mpu2 = new MPU6050; //create a temporary MPU connection for testing
majik 0:dceb852b19f3 86 mpu2->reset();
majik 0:dceb852b19f3 87 //bt.baud(baudRate);
majik 0:dceb852b19f3 88 while(mpu2->testConnection() == 0){
majik 0:dceb852b19f3 89 delete mpu2; //delete mpu2 so I can use PTE0 as a GPIO again
majik 0:dceb852b19f3 90 bt.baud(baudRate); //you must reset baud rate whenever a new MPU6050 is declared
majik 0:dceb852b19f3 91 DigitalOut sda_pinx(PTE0);
majik 0:dceb852b19f3 92 sda_pinx = 0; //set this to 0 to pull the SDA line down from 3.3V
majik 0:dceb852b19f3 93 reset_pin = 0; //make sure transistor = OFF
majik 0:dceb852b19f3 94 bt.printf("\n\rRetry");
majik 0:dceb852b19f3 95 wait_ms(50);
majik 0:dceb852b19f3 96 MPU6050 *mpu2 = new MPU6050;
majik 0:dceb852b19f3 97 wait_ms(500);
majik 0:dceb852b19f3 98 if(mpu2->testConnection() == 0)
majik 0:dceb852b19f3 99 {
majik 0:dceb852b19f3 100 reset_pin = 1;
majik 0:dceb852b19f3 101 wait_ms(50);
majik 0:dceb852b19f3 102 reset_pin = 0;
majik 0:dceb852b19f3 103 }
majik 0:dceb852b19f3 104 }
majik 0:dceb852b19f3 105 delete mpu2; //free the memory allocated to mpu2 (this is important)
majik 0:dceb852b19f3 106 }else{
majik 0:dceb852b19f3 107 bt.printf("\n\rSetting up I2C connection...");
majik 0:dceb852b19f3 108 reset_1 = 1;
majik 0:dceb852b19f3 109 reset_pin = 1;
majik 0:dceb852b19f3 110 wait_ms(20);
majik 0:dceb852b19f3 111 reset_pin = 0;
majik 0:dceb852b19f3 112 reset_1 = 0;
majik 0:dceb852b19f3 113 wait_ms(20);
majik 0:dceb852b19f3 114 }
majik 0:dceb852b19f3 115 reset_pin = 0;
majik 0:dceb852b19f3 116 DigitalIn not_used(PTE23);
majik 0:dceb852b19f3 117 myled = 0; //turn ON LED
majik 0:dceb852b19f3 118 return 1;
majik 0:dceb852b19f3 119 }
majik 0:dceb852b19f3 120
majik 0:dceb852b19f3 121
majik 0:dceb852b19f3 122 void start_dmp(MPU6050 mpu1){ //this will get the DMP ready
majik 0:dceb852b19f3 123 //initialize device
majik 0:dceb852b19f3 124 mpu1.reset();
majik 0:dceb852b19f3 125 bt.printf("\n\rInitializing I2C devices...");
majik 0:dceb852b19f3 126 devStatus = mpu1.dmpInitialize();
majik 0:dceb852b19f3 127 //insert gyro offsets here// Write a calibration algorithm in the future
majik 0:dceb852b19f3 128 mpu1.setXGyroOffset(-31); //800/25=32 //-31 is the largest offset available
majik 0:dceb852b19f3 129 mpu1.setYGyroOffset(-183/25);
majik 0:dceb852b19f3 130 mpu1.setZGyroOffset(80/25);
majik 0:dceb852b19f3 131 mpu1.setXAccelOffset(0); //the accel offsets don't do anything
majik 0:dceb852b19f3 132 mpu1.setYAccelOffset(0);
majik 0:dceb852b19f3 133 mpu1.setZAccelOffset(0);
majik 0:dceb852b19f3 134
majik 0:dceb852b19f3 135
majik 0:dceb852b19f3 136 if(devStatus == 0){ //this means initialize was successful
majik 0:dceb852b19f3 137 //turn on DMP
majik 0:dceb852b19f3 138 bt.printf("\n\rEnabling DMP");
majik 0:dceb852b19f3 139 mpu1.setDMPEnabled(true);
majik 0:dceb852b19f3 140
majik 0:dceb852b19f3 141 // enable interrupt detection
majik 0:dceb852b19f3 142 bt.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n");
majik 0:dceb852b19f3 143 checkpin.rise(&dmpDataReady);
majik 0:dceb852b19f3 144
majik 0:dceb852b19f3 145 mpuIntStatus = mpu1.getIntStatus();
majik 0:dceb852b19f3 146
majik 0:dceb852b19f3 147 //set the DMP ready flag so main loop knows when it is ok to use
majik 0:dceb852b19f3 148 bt.printf("DMP ready! Waiting for first interrupt");
majik 0:dceb852b19f3 149 dmpReady = true;
majik 0:dceb852b19f3 150
majik 0:dceb852b19f3 151 //get expected DMP packet size
majik 0:dceb852b19f3 152 packetSize = mpu1.dmpGetFIFOPacketSize();
majik 0:dceb852b19f3 153 }else{
majik 0:dceb852b19f3 154 bt.printf("\n\rDMP Initialization failed");
majik 0:dceb852b19f3 155 bt.printf("\t%d",devStatus);
majik 0:dceb852b19f3 156 wait_ms(1000);
majik 0:dceb852b19f3 157 }
majik 0:dceb852b19f3 158 }
majik 0:dceb852b19f3 159 void update_dmp(MPU6050 mpu1){
majik 0:dceb852b19f3 160
majik 0:dceb852b19f3 161 if (!dmpReady) return;
majik 0:dceb852b19f3 162 //while (!mpuInterrupt && fifoCount < packetSize) {
majik 0:dceb852b19f3 163 // other program behavior stuff here
majik 0:dceb852b19f3 164 // if you are really paranoid you can frequently test in between other
majik 0:dceb852b19f3 165 // stuff to see if mpuInterrupt is true, and if so, "break;" from the
majik 0:dceb852b19f3 166 // while() loop to immediately process the MPU data
majik 0:dceb852b19f3 167 // .
majik 0:dceb852b19f3 168 //}
majik 0:dceb852b19f3 169 // reset interrupt flag and get INT_STATUS byte
majik 0:dceb852b19f3 170 //mpuInterrupt = false; //this resets the interrupt flag
majik 0:dceb852b19f3 171 mpuIntStatus = mpu1.getIntStatus();
majik 0:dceb852b19f3 172
majik 0:dceb852b19f3 173 //get current FIFO count;
majik 0:dceb852b19f3 174 fifoCount = mpu1.getFIFOCount();
majik 0:dceb852b19f3 175
majik 0:dceb852b19f3 176 //check for overflow (only happens if your code sucks)
majik 0:dceb852b19f3 177 if((mpuIntStatus & 0x10) || fifoCount == 1024){
majik 0:dceb852b19f3 178 //reset so we can continue cleanly
majik 0:dceb852b19f3 179 mpu1.resetFIFO();
majik 0:dceb852b19f3 180 bt.printf("\n\rFIFO overflow");
majik 0:dceb852b19f3 181 } else if (mpuIntStatus & 0x02){
majik 0:dceb852b19f3 182 //wait for correct available data length (should be very short)
majik 0:dceb852b19f3 183 while (fifoCount < packetSize) fifoCount = mpu1.getFIFOCount();
majik 0:dceb852b19f3 184
majik 0:dceb852b19f3 185 //read a packet from FIFO
majik 0:dceb852b19f3 186 mpu1.getFIFOBytes(fifoBuffer, packetSize);
majik 0:dceb852b19f3 187
majik 0:dceb852b19f3 188 //track FIFO count here in the case there is > 1 packet available
majik 0:dceb852b19f3 189 //(allows us to immediately read more w/o waiting for interrupt)
majik 0:dceb852b19f3 190 fifoCount -= packetSize;
majik 0:dceb852b19f3 191
majik 0:dceb852b19f3 192 mpu1.dmpGetQuaternion(&q,fifoBuffer);
majik 0:dceb852b19f3 193 mpu1.dmpGetGravity(&gravity, &q);
majik 0:dceb852b19f3 194 mpu1.dmpGetYawPitchRoll(ypr,&q,&gravity);
majik 0:dceb852b19f3 195 #ifdef PRINT_GYR
majik 0:dceb852b19f3 196 bt.printf("\n\rypr\t %f\t %f\t %f",ypr[0]*180/PI,ypr[1]*180/PI,ypr[2]*180/PI);
majik 0:dceb852b19f3 197 #endif
majik 0:dceb852b19f3 198
majik 0:dceb852b19f3 199 mpu1.dmpGetAccel(&aa, fifoBuffer);
majik 0:dceb852b19f3 200 mpu1.dmpGetLinearAccel(&aaReal, &aa, &gravity);
majik 0:dceb852b19f3 201 #ifdef PRINT_ACC
majik 0:dceb852b19f3 202 bt.printf("\n\rareal\t%d\t%d\t%d",aaReal.x,aaReal.y,aaReal.z);
majik 0:dceb852b19f3 203 #endif
majik 0:dceb852b19f3 204 }
majik 0:dceb852b19f3 205 }