![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 | } |