This is for our FYDP project. 2 MPU6050s are used
DMP/DMP.cpp@2:7b3822eacad8, 2015-03-21 (annotated)
- Committer:
- majik
- Date:
- Sat Mar 21 22:44:36 2015 +0000
- Revision:
- 2:7b3822eacad8
- Parent:
- 0:21019d94ad33
When we connect 2 IMUs, one of them is laggy and the I2C connection fails after some random interval of time.; We are now only using one IMU.; Possible solution: Put both IMUs on the same I2C bus instead of separate I2C busses.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
majik | 0:21019d94ad33 | 1 | //Copied MPU6050.h and MPU6050.cpp from another library (otherwise MPU gets stuck randomly) |
majik | 0:21019d94ad33 | 2 | |
majik | 0:21019d94ad33 | 3 | /*** THIS IS THE BEST VERSION OF DMP CODE |
majik | 0:21019d94ad33 | 4 | This code demonstrates how to access the MPU6050 IMU and fix the |
majik | 0:21019d94ad33 | 5 | connection errors that occur, using a hardware hack. |
majik | 0:21019d94ad33 | 6 | It requires two hardware fixes: A transistor (PTE5) and an extra GPIO pin (PTE23) connected |
majik | 0:21019d94ad33 | 7 | to the SDA line. |
majik | 0:21019d94ad33 | 8 | Error 1: The SDA line gets stuck on ground. |
majik | 0:21019d94ad33 | 9 | Solution 1: Use the transistor to raise the SDA voltage off of ground. This resets the I2C bus. (line 71) |
majik | 0:21019d94ad33 | 10 | Error 2: The SDA line gets stuck on 3.3V |
majik | 0:21019d94ad33 | 11 | Solution 2: Use the GPIO line to pull the SDA voltage down a bit to reset the I2C bus. (line 96) |
majik | 0:21019d94ad33 | 12 | (I am currently trying to find a software solution for the above hack) |
majik | 0:21019d94ad33 | 13 | |
majik | 0:21019d94ad33 | 14 | This code works for using the DMP. (See "DMP" for implementation that can do raw accel data) |
majik | 0:21019d94ad33 | 15 | It appears all connection issues have been fixed. |
majik | 0:21019d94ad33 | 16 | Next step: make code neater (in progress) |
majik | 0:21019d94ad33 | 17 | |
majik | 0:21019d94ad33 | 18 | LEARNED: Do not need the PTE23 line. Only fix necessary is the transistor (PTE5) |
majik | 0:21019d94ad33 | 19 | ***/ |
majik | 0:21019d94ad33 | 20 | |
majik | 0:21019d94ad33 | 21 | #include "mbed.h" |
majik | 0:21019d94ad33 | 22 | #include "DMP.h" |
majik | 0:21019d94ad33 | 23 | |
majik | 0:21019d94ad33 | 24 | |
majik | 0:21019d94ad33 | 25 | //******* MPU DECLARATION THINGS *****************// |
majik | 0:21019d94ad33 | 26 | int dmp_test = 1; //if this is 0, get raw MPU values |
majik | 0:21019d94ad33 | 27 | //if this is 1, get DMP values |
majik | 0:21019d94ad33 | 28 | |
majik | 0:21019d94ad33 | 29 | int acc_offset[3]; //this saves the offset for acceleration |
majik | 0:21019d94ad33 | 30 | |
majik | 0:21019d94ad33 | 31 | int16_t ax, ay, az; |
majik | 0:21019d94ad33 | 32 | int16_t gx, gy, gz; |
majik | 0:21019d94ad33 | 33 | |
majik | 0:21019d94ad33 | 34 | //******* DMP things ****************// |
majik | 0:21019d94ad33 | 35 | // MPU control/status vars |
majik | 0:21019d94ad33 | 36 | bool dmpReady = false; // set true if DMP init was succesfful |
majik | 0:21019d94ad33 | 37 | uint8_t mpuIntStatus; // holds interrupt status byte from MPU |
majik | 0:21019d94ad33 | 38 | uint8_t devStatus; // return status after each device operation (0 = success) |
majik | 0:21019d94ad33 | 39 | uint16_t packetSize; // expected DMP packet size (default is 42 bytes) |
majik | 0:21019d94ad33 | 40 | uint16_t fifoCount; // count of all bytes currently in FIFO |
majik | 0:21019d94ad33 | 41 | uint8_t fifoBuffer[64]; // FIFO storage buffer |
majik | 0:21019d94ad33 | 42 | bool dmpReady2 = false; // set true if DMP init was succesfful |
majik | 0:21019d94ad33 | 43 | uint8_t mpuIntStatus2; // holds interrupt status byte from MPU |
majik | 0:21019d94ad33 | 44 | uint8_t devStatus2; // return status after each device operation (0 = success) |
majik | 0:21019d94ad33 | 45 | uint16_t packetSize2; // expected DMP packet size (default is 42 bytes) |
majik | 0:21019d94ad33 | 46 | uint16_t fifoCount2; // count of all bytes currently in FIFO |
majik | 0:21019d94ad33 | 47 | uint8_t fifoBuffer2[64]; // FIFO storage buffer |
majik | 0:21019d94ad33 | 48 | |
majik | 0:21019d94ad33 | 49 | |
majik | 0:21019d94ad33 | 50 | |
majik | 0:21019d94ad33 | 51 | uint8_t MPU_CONNECTION; |
majik | 0:21019d94ad33 | 52 | |
majik | 0:21019d94ad33 | 53 | // orientation/motion vars |
majik | 0:21019d94ad33 | 54 | Quaternion q; // [w,x,y,z] quaternion container |
majik | 2:7b3822eacad8 | 55 | Quaternion q2; // [w,x,y,z] quaternion container |
majik | 0:21019d94ad33 | 56 | VectorInt16 aa; // [x,y,z] accel sensor measurements |
majik | 0:21019d94ad33 | 57 | VectorInt16 aaReal; // [x,y,z] gravity-free accel sensor measurements |
majik | 0:21019d94ad33 | 58 | VectorInt16 aaWorld; // [x,y,z] world-frame accel sensor measurements |
majik | 0:21019d94ad33 | 59 | VectorFloat gravity; // [x,y,z] gravity vector |
majik | 2:7b3822eacad8 | 60 | VectorFloat gravity2; // [x,y,z] gravity vector |
majik | 2:7b3822eacad8 | 61 | |
majik | 0:21019d94ad33 | 62 | float euler[3]; // [psi,theta,phi] Euler angle container |
majik | 0:21019d94ad33 | 63 | float ypr[3]; // [yaw,pitch,roll] yaw/pitch/roll container [rad]. Multiply by 180, divide by PI for [deg] |
majik | 2:7b3822eacad8 | 64 | float ypr2[3]; |
majik | 0:21019d94ad33 | 65 | InterruptIn checkpin(PTD7); //interrupt |
majik | 0:21019d94ad33 | 66 | InterruptIn checkpin2(PTD5); //PTD5 |
majik | 0:21019d94ad33 | 67 | //*** Interrupt Detection Routine ***// |
majik | 0:21019d94ad33 | 68 | volatile bool mpuInterrupt = false; //indicates whether interrupt pin has gone high |
majik | 0:21019d94ad33 | 69 | void dmpDataReady(){ |
majik | 0:21019d94ad33 | 70 | mpuInterrupt = true; |
majik | 0:21019d94ad33 | 71 | } |
majik | 0:21019d94ad33 | 72 | volatile bool mpuInterrupt2 = false; //indicates whether interrupt pin has gone high |
majik | 0:21019d94ad33 | 73 | void dmpDataReady2(){ |
majik | 0:21019d94ad33 | 74 | mpuInterrupt2 = true; |
majik | 0:21019d94ad33 | 75 | } |
majik | 0:21019d94ad33 | 76 | |
majik | 0:21019d94ad33 | 77 | //****** END MPU DECLARATION ********************// |
majik | 0:21019d94ad33 | 78 | |
majik | 0:21019d94ad33 | 79 | |
majik | 0:21019d94ad33 | 80 | int test_dmp(); //this wraps up the code req'd to start the DMP |
majik | 0:21019d94ad33 | 81 | void start_dmp(MPU6050); //this will get the DMP ready |
majik | 0:21019d94ad33 | 82 | void update_dmp(); //call this frequently |
majik | 0:21019d94ad33 | 83 | |
majik | 0:21019d94ad33 | 84 | //*********************FUNCTIONS***************************************************************// |
majik | 0:21019d94ad33 | 85 | int test_dmp(){ //returns 1 if ok (right now, it doesn't return at all if there is a problem |
majik | 0:21019d94ad33 | 86 | imuSwitch = 1; |
majik | 0:21019d94ad33 | 87 | LED_OFF; |
majik | 0:21019d94ad33 | 88 | Thread::wait(100); |
majik | 0:21019d94ad33 | 89 | |
majik | 0:21019d94ad33 | 90 | while(mpu.testConnection()==0) |
majik | 0:21019d94ad33 | 91 | { |
majik | 0:21019d94ad33 | 92 | imuSwitch = 0; |
majik | 0:21019d94ad33 | 93 | Thread::wait(100); |
majik | 0:21019d94ad33 | 94 | imuSwitch = 1; |
majik | 0:21019d94ad33 | 95 | Thread::wait(100); |
majik | 0:21019d94ad33 | 96 | bt.lock(); |
majik | 0:21019d94ad33 | 97 | bt.printf("Stuck on IMU1"); |
majik | 0:21019d94ad33 | 98 | bt.unlock(); |
majik | 0:21019d94ad33 | 99 | } |
majik | 0:21019d94ad33 | 100 | imuSwitch = 1; |
majik | 0:21019d94ad33 | 101 | LED_ON //turn ON LED |
majik | 0:21019d94ad33 | 102 | return 1; |
majik | 0:21019d94ad33 | 103 | } |
majik | 0:21019d94ad33 | 104 | int test_dmp2(){ //returns 1 if ok (right now, it doesn't return at all if there is a problem |
majik | 0:21019d94ad33 | 105 | imu2Switch = 1; |
majik | 0:21019d94ad33 | 106 | LED_OFF; |
majik | 0:21019d94ad33 | 107 | Thread::wait(100); |
majik | 0:21019d94ad33 | 108 | |
majik | 0:21019d94ad33 | 109 | while(mpu2.testConnection()==0) |
majik | 0:21019d94ad33 | 110 | { |
majik | 0:21019d94ad33 | 111 | imu2Switch = 0; |
majik | 0:21019d94ad33 | 112 | Thread::wait(100); |
majik | 0:21019d94ad33 | 113 | imu2Switch = 1; |
majik | 0:21019d94ad33 | 114 | Thread::wait(100); |
majik | 0:21019d94ad33 | 115 | bt.lock(); |
majik | 0:21019d94ad33 | 116 | bt.printf("Stuck on IMU2"); |
majik | 0:21019d94ad33 | 117 | bt.unlock(); |
majik | 0:21019d94ad33 | 118 | } |
majik | 0:21019d94ad33 | 119 | imu2Switch = 1; |
majik | 0:21019d94ad33 | 120 | LED_ON //turn ON LED |
majik | 0:21019d94ad33 | 121 | return 1; |
majik | 0:21019d94ad33 | 122 | } |
majik | 0:21019d94ad33 | 123 | |
majik | 0:21019d94ad33 | 124 | void start_dmp(MPU6050 mpu1){ //this will get the DMP ready |
majik | 0:21019d94ad33 | 125 | //initialize device |
majik | 0:21019d94ad33 | 126 | mpu1.reset(); |
majik | 0:21019d94ad33 | 127 | DMP_DBG_MSG("\n\rInitializing I2C devices...") |
majik | 0:21019d94ad33 | 128 | devStatus = mpu1.dmpInitialize(); |
majik | 0:21019d94ad33 | 129 | //insert gyro offsets here// Write a calibration algorithm in the future |
majik | 0:21019d94ad33 | 130 | mpu1.setXGyroOffset(0); //800/25=32 //-31 is the largest offset available |
majik | 0:21019d94ad33 | 131 | mpu1.setYGyroOffset(0); |
majik | 0:21019d94ad33 | 132 | mpu1.setZGyroOffset(0); |
majik | 0:21019d94ad33 | 133 | mpu1.setXAccelOffset(0); //the accel offsets don't do anything |
majik | 0:21019d94ad33 | 134 | mpu1.setYAccelOffset(0); |
majik | 0:21019d94ad33 | 135 | mpu1.setZAccelOffset(0); |
majik | 0:21019d94ad33 | 136 | |
majik | 0:21019d94ad33 | 137 | mpu1.setFullScaleGyroRange(MPU6050_GYRO_FS_2000); |
majik | 0:21019d94ad33 | 138 | |
majik | 0:21019d94ad33 | 139 | if(devStatus == 0){ //this means initialize was successful |
majik | 0:21019d94ad33 | 140 | //turn on DMP |
majik | 0:21019d94ad33 | 141 | DMP_DBG_MSG("\n\rEnabling DMP") |
majik | 0:21019d94ad33 | 142 | mpu1.setDMPEnabled(true); |
majik | 0:21019d94ad33 | 143 | |
majik | 0:21019d94ad33 | 144 | // enable interrupt detection |
majik | 0:21019d94ad33 | 145 | DMP_DBG_MSG("Enabling interrupt detection (Arduino external interrupt 0)...\r\n") |
majik | 0:21019d94ad33 | 146 | checkpin.rise(&dmpDataReady); |
majik | 0:21019d94ad33 | 147 | |
majik | 0:21019d94ad33 | 148 | mpuIntStatus = mpu1.getIntStatus(); |
majik | 0:21019d94ad33 | 149 | |
majik | 0:21019d94ad33 | 150 | //set the DMP ready flag so main loop knows when it is ok to use |
majik | 0:21019d94ad33 | 151 | DMP_DBG_MSG("DMP ready! Waiting for first interrupt") |
majik | 0:21019d94ad33 | 152 | dmpReady = true; |
majik | 0:21019d94ad33 | 153 | |
majik | 0:21019d94ad33 | 154 | //get expected DMP packet size |
majik | 0:21019d94ad33 | 155 | packetSize = mpu1.dmpGetFIFOPacketSize(); |
majik | 0:21019d94ad33 | 156 | }else{ |
majik | 0:21019d94ad33 | 157 | DMP_DBG_MSG("\n\rDMP Initialization failed") |
majik | 0:21019d94ad33 | 158 | DMP_DBG_MSG("\t%d",devStatus) |
majik | 0:21019d94ad33 | 159 | Thread::wait(1000); |
majik | 0:21019d94ad33 | 160 | } |
majik | 0:21019d94ad33 | 161 | acc_offset[0]=0; |
majik | 0:21019d94ad33 | 162 | acc_offset[1]=0; |
majik | 0:21019d94ad33 | 163 | acc_offset[2]=0; |
majik | 0:21019d94ad33 | 164 | mpu.setDLPFMode(MPU6050_DLPF_BW_42); |
majik | 0:21019d94ad33 | 165 | } |
majik | 0:21019d94ad33 | 166 | void start_dmp2(MPU6051 mpu1){ //this will get the DMP ready |
majik | 0:21019d94ad33 | 167 | //initialize device |
majik | 0:21019d94ad33 | 168 | mpu2.reset(); |
majik | 0:21019d94ad33 | 169 | DMP_DBG_MSG("\n\rInitializing I2C devices...") |
majik | 0:21019d94ad33 | 170 | devStatus2 = mpu2.dmpInitialize(); |
majik | 0:21019d94ad33 | 171 | //insert gyro offsets here// Write a calibration algorithm in the future |
majik | 0:21019d94ad33 | 172 | mpu2.setXGyroOffset(0); //800/25=32 //-31 is the largest offset available |
majik | 0:21019d94ad33 | 173 | mpu2.setYGyroOffset(0); |
majik | 0:21019d94ad33 | 174 | mpu2.setZGyroOffset(0); |
majik | 0:21019d94ad33 | 175 | mpu2.setXAccelOffset(0); //the accel offsets don't do anything |
majik | 0:21019d94ad33 | 176 | mpu2.setYAccelOffset(0); |
majik | 0:21019d94ad33 | 177 | mpu2.setZAccelOffset(0); |
majik | 0:21019d94ad33 | 178 | |
majik | 0:21019d94ad33 | 179 | mpu2.setFullScaleGyroRange(MPU6050_GYRO_FS_2000); |
majik | 0:21019d94ad33 | 180 | |
majik | 0:21019d94ad33 | 181 | if(devStatus2 == 0){ //this means initialize was successful |
majik | 0:21019d94ad33 | 182 | //turn on DMP |
majik | 0:21019d94ad33 | 183 | DMP_DBG_MSG("\n\rEnabling DMP") |
majik | 0:21019d94ad33 | 184 | mpu2.setDMPEnabled(true); |
majik | 0:21019d94ad33 | 185 | |
majik | 0:21019d94ad33 | 186 | // enable interrupt detection |
majik | 0:21019d94ad33 | 187 | DMP_DBG_MSG("Enabling interrupt detection (Arduino external interrupt 0)...\r\n") |
majik | 0:21019d94ad33 | 188 | checkpin2.rise(&dmpDataReady2); |
majik | 0:21019d94ad33 | 189 | |
majik | 0:21019d94ad33 | 190 | mpuIntStatus2 = mpu2.getIntStatus(); |
majik | 0:21019d94ad33 | 191 | |
majik | 0:21019d94ad33 | 192 | //set the DMP ready flag so main loop knows when it is ok to use |
majik | 0:21019d94ad33 | 193 | DMP_DBG_MSG("DMP ready! Waiting for first interrupt") |
majik | 0:21019d94ad33 | 194 | dmpReady2 = true; |
majik | 0:21019d94ad33 | 195 | |
majik | 0:21019d94ad33 | 196 | //get expected DMP packet size |
majik | 0:21019d94ad33 | 197 | packetSize2 = mpu2.dmpGetFIFOPacketSize(); |
majik | 0:21019d94ad33 | 198 | }else{ |
majik | 0:21019d94ad33 | 199 | DMP_DBG_MSG("\n\rDMP Initialization failed") |
majik | 0:21019d94ad33 | 200 | DMP_DBG_MSG("\t%d",devStatus2) |
majik | 0:21019d94ad33 | 201 | Thread::wait(1000); |
majik | 0:21019d94ad33 | 202 | } |
majik | 0:21019d94ad33 | 203 | acc_offset[0]=0; |
majik | 0:21019d94ad33 | 204 | acc_offset[1]=0; |
majik | 0:21019d94ad33 | 205 | acc_offset[2]=0; |
majik | 0:21019d94ad33 | 206 | mpu2.setDLPFMode(MPU6050_DLPF_BW_42); |
majik | 0:21019d94ad33 | 207 | } |
majik | 0:21019d94ad33 | 208 | |
majik | 0:21019d94ad33 | 209 | void update_dmp(){ |
majik | 0:21019d94ad33 | 210 | if (!dmpReady) return; |
majik | 0:21019d94ad33 | 211 | while (!mpuInterrupt && fifoCount < packetSize) { |
majik | 0:21019d94ad33 | 212 | // other program behavior stuff here |
majik | 0:21019d94ad33 | 213 | // if you are really paranoid you can frequently test in between other |
majik | 0:21019d94ad33 | 214 | // stuff to see if mpuInterrupt is true, and if so, "break;" from the |
majik | 0:21019d94ad33 | 215 | // while() loop to immediately process the MPU data |
majik | 0:21019d94ad33 | 216 | // . |
majik | 0:21019d94ad33 | 217 | } |
majik | 0:21019d94ad33 | 218 | // reset interrupt flag and get INT_STATUS byte |
majik | 0:21019d94ad33 | 219 | mpuInterrupt = false; //this resets the interrupt flag |
majik | 0:21019d94ad33 | 220 | mpuIntStatus = mpu.getIntStatus(); |
majik | 0:21019d94ad33 | 221 | |
majik | 0:21019d94ad33 | 222 | //get current FIFO count; |
majik | 0:21019d94ad33 | 223 | fifoCount = mpu.getFIFOCount(); |
majik | 0:21019d94ad33 | 224 | |
majik | 0:21019d94ad33 | 225 | //check for overflow (only happens if your code sucks) |
majik | 0:21019d94ad33 | 226 | if((mpuIntStatus & 0x10) || fifoCount == 1024){ |
majik | 0:21019d94ad33 | 227 | //reset so we can continue cleanly |
majik | 0:21019d94ad33 | 228 | mpu.resetFIFO(); |
majik | 0:21019d94ad33 | 229 | DMP_DBG_MSG("\n\rFIFO overflow") |
majik | 0:21019d94ad33 | 230 | } else if (mpuIntStatus & 0x02){ |
majik | 0:21019d94ad33 | 231 | int16_t x,y,z; |
majik | 0:21019d94ad33 | 232 | //wait for correct available data length (should be very short) |
majik | 0:21019d94ad33 | 233 | while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); |
majik | 0:21019d94ad33 | 234 | |
majik | 0:21019d94ad33 | 235 | //read a packet from FIFO |
majik | 0:21019d94ad33 | 236 | mpu.getFIFOBytes(fifoBuffer, packetSize); |
majik | 0:21019d94ad33 | 237 | |
majik | 0:21019d94ad33 | 238 | //track FIFO count here in the case there is > 1 packet available |
majik | 0:21019d94ad33 | 239 | //(allows us to immediately read more w/o waiting for interrupt) |
majik | 0:21019d94ad33 | 240 | fifoCount -= packetSize; |
majik | 0:21019d94ad33 | 241 | |
majik | 0:21019d94ad33 | 242 | mpu.dmpGetQuaternion(&q,fifoBuffer); |
majik | 0:21019d94ad33 | 243 | imu_data.quat[0]=q.w; |
majik | 0:21019d94ad33 | 244 | imu_data.quat[1]=q.x; |
majik | 0:21019d94ad33 | 245 | imu_data.quat[2]=q.y; |
majik | 0:21019d94ad33 | 246 | imu_data.quat[3]=q.z; |
majik | 0:21019d94ad33 | 247 | mpu.dmpGetGravity(&gravity, &q); |
majik | 0:21019d94ad33 | 248 | mpu.dmpGetYawPitchRoll(ypr,&q,&gravity); |
majik | 0:21019d94ad33 | 249 | imu_data.ypr[0] = ypr[0]*180/M_PI;//MARK - this saves the yaw data so everyone can access it |
majik | 0:21019d94ad33 | 250 | imu_data.ypr[1] = ypr[1]*180/M_PI; |
majik | 0:21019d94ad33 | 251 | imu_data.ypr[2] = ypr[2]*180/M_PI; |
majik | 0:21019d94ad33 | 252 | |
majik | 0:21019d94ad33 | 253 | mpu.getAcceleration(&x,&y,&z); |
majik | 0:21019d94ad33 | 254 | imu_data.acc[0] = (x-acc_offset[0]) *9.81/16384; |
majik | 0:21019d94ad33 | 255 | imu_data.acc[1] = (y-acc_offset[1]) *9.81/16384; |
majik | 0:21019d94ad33 | 256 | imu_data.acc[2] = (z-acc_offset[2]) *9.81/16384; |
majik | 0:21019d94ad33 | 257 | #ifdef PRINT_GYR |
majik | 0:21019d94ad33 | 258 | DMP_DBG_MSG("\n\rypr\t %f\t %f\t %f",ypr[0]*180/PI,ypr[1]*180/PI,ypr[2]*180/PI) |
majik | 0:21019d94ad33 | 259 | #endif |
majik | 0:21019d94ad33 | 260 | |
majik | 0:21019d94ad33 | 261 | mpu.dmpGetAccel(&aa, fifoBuffer); |
majik | 0:21019d94ad33 | 262 | mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); |
majik | 0:21019d94ad33 | 263 | #ifdef PRINT_ACC |
majik | 0:21019d94ad33 | 264 | DMP_DBG_MSG("\n\rareal\t%d\t%d\t%d",aaReal.x,aaReal.y,aaReal.z) |
majik | 0:21019d94ad33 | 265 | #endif |
majik | 0:21019d94ad33 | 266 | //get rotations |
majik | 0:21019d94ad33 | 267 | mpu.getMotion6(&x,&y,&z,&gx,&gy,&gz); |
majik | 0:21019d94ad33 | 268 | float scale = 250/16384; |
majik | 0:21019d94ad33 | 269 | imu_data.gyr[0] = gx*scale; |
majik | 0:21019d94ad33 | 270 | imu_data.gyr[1] = gy*scale; |
majik | 0:21019d94ad33 | 271 | imu_data.gyr[2] = gz*scale; |
majik | 0:21019d94ad33 | 272 | |
majik | 0:21019d94ad33 | 273 | } |
majik | 0:21019d94ad33 | 274 | } |
majik | 0:21019d94ad33 | 275 | void update_dmp2(){ |
majik | 0:21019d94ad33 | 276 | if (!dmpReady2) return; |
majik | 0:21019d94ad33 | 277 | while (!mpuInterrupt2 && fifoCount2 < packetSize2) { |
majik | 0:21019d94ad33 | 278 | // other program behavior stuff here |
majik | 0:21019d94ad33 | 279 | // if you are really paranoid you can frequently test in between other |
majik | 0:21019d94ad33 | 280 | // stuff to see if mpuInterrupt is true, and if so, "break;" from the |
majik | 0:21019d94ad33 | 281 | // while() loop to immediately process the MPU data |
majik | 0:21019d94ad33 | 282 | // . |
majik | 0:21019d94ad33 | 283 | } |
majik | 0:21019d94ad33 | 284 | // reset interrupt flag and get INT_STATUS byte |
majik | 0:21019d94ad33 | 285 | mpuInterrupt2 = false; //this resets the interrupt flag |
majik | 0:21019d94ad33 | 286 | mpuIntStatus2 = mpu2.getIntStatus(); |
majik | 0:21019d94ad33 | 287 | |
majik | 0:21019d94ad33 | 288 | //get current FIFO count; |
majik | 0:21019d94ad33 | 289 | fifoCount2 = mpu2.getFIFOCount(); |
majik | 0:21019d94ad33 | 290 | |
majik | 0:21019d94ad33 | 291 | //check for overflow (only happens if your code sucks) |
majik | 0:21019d94ad33 | 292 | if((mpuIntStatus2 & 0x10) || fifoCount2 == 1024){ |
majik | 0:21019d94ad33 | 293 | //reset so we can continue cleanly |
majik | 0:21019d94ad33 | 294 | mpu2.resetFIFO(); |
majik | 0:21019d94ad33 | 295 | DMP_DBG_MSG("\n\rFIFO overflow") |
majik | 0:21019d94ad33 | 296 | } else if (mpuIntStatus2 & 0x02){ |
majik | 0:21019d94ad33 | 297 | int16_t x,y,z; |
majik | 0:21019d94ad33 | 298 | //wait for correct available data length (should be very short) |
majik | 0:21019d94ad33 | 299 | while (fifoCount2 < packetSize2) fifoCount2 = mpu2.getFIFOCount(); |
majik | 0:21019d94ad33 | 300 | |
majik | 0:21019d94ad33 | 301 | //read a packet from FIFO |
majik | 0:21019d94ad33 | 302 | mpu2.getFIFOBytes(fifoBuffer2, packetSize2); |
majik | 0:21019d94ad33 | 303 | |
majik | 0:21019d94ad33 | 304 | //track FIFO count here in the case there is > 1 packet available |
majik | 0:21019d94ad33 | 305 | //(allows us to immediately read more w/o waiting for interrupt) |
majik | 0:21019d94ad33 | 306 | fifoCount2 -= packetSize2; |
majik | 0:21019d94ad33 | 307 | |
majik | 2:7b3822eacad8 | 308 | mpu2.dmpGetQuaternion(&q2,fifoBuffer2); |
majik | 2:7b3822eacad8 | 309 | imu2_data.quat[0]=q2.w; |
majik | 2:7b3822eacad8 | 310 | imu2_data.quat[1]=q2.x; |
majik | 2:7b3822eacad8 | 311 | imu2_data.quat[2]=q2.y; |
majik | 2:7b3822eacad8 | 312 | imu2_data.quat[3]=q2.z; |
majik | 2:7b3822eacad8 | 313 | mpu2.dmpGetGravity(&gravity2, &q2); |
majik | 2:7b3822eacad8 | 314 | mpu2.dmpGetYawPitchRoll(ypr2,&q2,&gravity2); |
majik | 2:7b3822eacad8 | 315 | imu2_data.ypr[0] = ypr2[0]*180/M_PI;//MARK - this saves the yaw data so everyone can access it |
majik | 2:7b3822eacad8 | 316 | imu2_data.ypr[1] = ypr2[1]*180/M_PI; |
majik | 2:7b3822eacad8 | 317 | imu2_data.ypr[2] = ypr2[2]*180/M_PI; |
majik | 0:21019d94ad33 | 318 | |
majik | 0:21019d94ad33 | 319 | mpu2.getAcceleration(&x,&y,&z); |
majik | 0:21019d94ad33 | 320 | imu2_data.acc[0] = (x-acc_offset[0]) *9.81/16384; |
majik | 0:21019d94ad33 | 321 | imu2_data.acc[1] = (y-acc_offset[1]) *9.81/16384; |
majik | 0:21019d94ad33 | 322 | imu2_data.acc[2] = (z-acc_offset[2]) *9.81/16384; |
majik | 0:21019d94ad33 | 323 | #ifdef PRINT_GYR |
majik | 2:7b3822eacad8 | 324 | DMP_DBG_MSG("\n\rypr\t %f\t %f\t %f",ypr2[0]*180/PI,ypr2[1]*180/PI,ypr2[2]*180/PI) |
majik | 0:21019d94ad33 | 325 | #endif |
majik | 0:21019d94ad33 | 326 | |
majik | 0:21019d94ad33 | 327 | mpu2.dmpGetAccel(&aa, fifoBuffer2); |
majik | 2:7b3822eacad8 | 328 | mpu2.dmpGetLinearAccel(&aaReal, &aa, &gravity2); |
majik | 0:21019d94ad33 | 329 | #ifdef PRINT_ACC |
majik | 0:21019d94ad33 | 330 | DMP_DBG_MSG("\n\rareal\t%d\t%d\t%d",aaReal.x,aaReal.y,aaReal.z) |
majik | 0:21019d94ad33 | 331 | #endif |
majik | 0:21019d94ad33 | 332 | //get rotations |
majik | 0:21019d94ad33 | 333 | mpu2.getMotion6(&x,&y,&z,&gx,&gy,&gz); |
majik | 0:21019d94ad33 | 334 | float scale = 250/16384; |
majik | 0:21019d94ad33 | 335 | imu2_data.gyr[0] = gx*scale; |
majik | 0:21019d94ad33 | 336 | imu2_data.gyr[1] = gy*scale; |
majik | 0:21019d94ad33 | 337 | imu2_data.gyr[2] = gz*scale; |
majik | 0:21019d94ad33 | 338 | |
majik | 0:21019d94ad33 | 339 | } |
majik | 0:21019d94ad33 | 340 | } |
majik | 0:21019d94ad33 | 341 | void update_acc() |
majik | 0:21019d94ad33 | 342 | { |
majik | 0:21019d94ad33 | 343 | int16_t x,y,z; |
majik | 0:21019d94ad33 | 344 | mpu.getAcceleration(&x,&y,&z); |
majik | 0:21019d94ad33 | 345 | imu_data.acc[0] = (x-acc_offset[0]) *9.81/16384; |
majik | 0:21019d94ad33 | 346 | imu_data.acc[1] = (y-acc_offset[1]) *9.81/16384; |
majik | 0:21019d94ad33 | 347 | imu_data.acc[2] = (z-acc_offset[2]) *9.81/16384; |
majik | 0:21019d94ad33 | 348 | } |
majik | 0:21019d94ad33 | 349 | void calibrate_1() //call this to calibrate the accelerometer |
majik | 0:21019d94ad33 | 350 | { //in the future, modify the OFFSET REGISTERS to make the DMP converge faster |
majik | 0:21019d94ad33 | 351 | //right now, I am only calculating the offset for the accelerometers |
majik | 0:21019d94ad33 | 352 | int sum[3] = {0,0,0}; |
majik | 0:21019d94ad33 | 353 | int i; |
majik | 0:21019d94ad33 | 354 | int16_t x,y,z; |
majik | 0:21019d94ad33 | 355 | |
majik | 0:21019d94ad33 | 356 | int count=0; |
majik | 0:21019d94ad33 | 357 | int t_now = t.read_ms(); |
majik | 0:21019d94ad33 | 358 | while((t.read_ms()-t_now) < 1000) //loop for one second |
majik | 0:21019d94ad33 | 359 | { |
majik | 0:21019d94ad33 | 360 | mpu.getAcceleration(&x,&y,&z); |
majik | 0:21019d94ad33 | 361 | sum[0] += x; |
majik | 0:21019d94ad33 | 362 | sum[1] += y; |
majik | 0:21019d94ad33 | 363 | sum[2] += z; |
majik | 0:21019d94ad33 | 364 | Thread::wait(5); |
majik | 0:21019d94ad33 | 365 | count++; |
majik | 0:21019d94ad33 | 366 | } |
majik | 0:21019d94ad33 | 367 | for(i = 0; i<3; i++) |
majik | 0:21019d94ad33 | 368 | acc_offset[i] = (sum[i]/count); |
majik | 0:21019d94ad33 | 369 | //bt.printf("ACC_OFF:%d;%d;%d;",acc_offset[0],acc_offset[1],acc_offset[2]);//print the offset used by accelerometer |
majik | 0:21019d94ad33 | 370 | } |