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