Mark Vandermeulen / FYDP_Final2

This is for our FYDP project. 2 MPU6050s are used

Dependencies:   Servo mbed

DMP/DMP.cpp@0:21019d94ad33, 2015-03-21 (annotated)

Committer:
majik
Date:
Sat Mar 21 21:31:29 2015 +0000
Revision:
0:21019d94ad33
Child:
2:7b3822eacad8
Both IMUs work now

Who changed what in which revision?

UserRevisionLine numberNew 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 0:21019d94ad33 55 VectorInt16 aa; // [x,y,z] accel sensor measurements
majik 0:21019d94ad33 56 VectorInt16 aaReal; // [x,y,z] gravity-free accel sensor measurements
majik 0:21019d94ad33 57 VectorInt16 aaWorld; // [x,y,z] world-frame accel sensor measurements
majik 0:21019d94ad33 58 VectorFloat gravity; // [x,y,z] gravity vector
majik 0:21019d94ad33 59 float euler[3]; // [psi,theta,phi] Euler angle container
majik 0:21019d94ad33 60 float ypr[3]; // [yaw,pitch,roll] yaw/pitch/roll container [rad]. Multiply by 180, divide by PI for [deg]
majik 0:21019d94ad33 61 InterruptIn checkpin(PTD7); //interrupt
majik 0:21019d94ad33 62 InterruptIn checkpin2(PTD5); //PTD5
majik 0:21019d94ad33 63 //*** Interrupt Detection Routine ***//
majik 0:21019d94ad33 64 volatile bool mpuInterrupt = false; //indicates whether interrupt pin has gone high
majik 0:21019d94ad33 65 void dmpDataReady(){
majik 0:21019d94ad33 66 mpuInterrupt = true;
majik 0:21019d94ad33 67 }
majik 0:21019d94ad33 68 volatile bool mpuInterrupt2 = false; //indicates whether interrupt pin has gone high
majik 0:21019d94ad33 69 void dmpDataReady2(){
majik 0:21019d94ad33 70 mpuInterrupt2 = true;
majik 0:21019d94ad33 71 }
majik 0:21019d94ad33 72
majik 0:21019d94ad33 73 //****** END MPU DECLARATION ********************//
majik 0:21019d94ad33 74
majik 0:21019d94ad33 75
majik 0:21019d94ad33 76 int test_dmp(); //this wraps up the code req'd to start the DMP
majik 0:21019d94ad33 77 void start_dmp(MPU6050); //this will get the DMP ready
majik 0:21019d94ad33 78 void update_dmp(); //call this frequently
majik 0:21019d94ad33 79
majik 0:21019d94ad33 80 //*********************FUNCTIONS***************************************************************//
majik 0:21019d94ad33 81 int test_dmp(){ //returns 1 if ok (right now, it doesn't return at all if there is a problem
majik 0:21019d94ad33 82 imuSwitch = 1;
majik 0:21019d94ad33 83 LED_OFF;
majik 0:21019d94ad33 84 Thread::wait(100);
majik 0:21019d94ad33 85
majik 0:21019d94ad33 86 while(mpu.testConnection()==0)
majik 0:21019d94ad33 87 {
majik 0:21019d94ad33 88 imuSwitch = 0;
majik 0:21019d94ad33 89 Thread::wait(100);
majik 0:21019d94ad33 90 imuSwitch = 1;
majik 0:21019d94ad33 91 Thread::wait(100);
majik 0:21019d94ad33 92 bt.lock();
majik 0:21019d94ad33 93 bt.printf("Stuck on IMU1");
majik 0:21019d94ad33 94 bt.unlock();
majik 0:21019d94ad33 95 }
majik 0:21019d94ad33 96 imuSwitch = 1;
majik 0:21019d94ad33 97 LED_ON //turn ON LED
majik 0:21019d94ad33 98 return 1;
majik 0:21019d94ad33 99 }
majik 0:21019d94ad33 100 int test_dmp2(){ //returns 1 if ok (right now, it doesn't return at all if there is a problem
majik 0:21019d94ad33 101 imu2Switch = 1;
majik 0:21019d94ad33 102 LED_OFF;
majik 0:21019d94ad33 103 Thread::wait(100);
majik 0:21019d94ad33 104
majik 0:21019d94ad33 105 while(mpu2.testConnection()==0)
majik 0:21019d94ad33 106 {
majik 0:21019d94ad33 107 imu2Switch = 0;
majik 0:21019d94ad33 108 Thread::wait(100);
majik 0:21019d94ad33 109 imu2Switch = 1;
majik 0:21019d94ad33 110 Thread::wait(100);
majik 0:21019d94ad33 111 bt.lock();
majik 0:21019d94ad33 112 bt.printf("Stuck on IMU2");
majik 0:21019d94ad33 113 bt.unlock();
majik 0:21019d94ad33 114 }
majik 0:21019d94ad33 115 imu2Switch = 1;
majik 0:21019d94ad33 116 LED_ON //turn ON LED
majik 0:21019d94ad33 117 return 1;
majik 0:21019d94ad33 118 }
majik 0:21019d94ad33 119
majik 0:21019d94ad33 120 void start_dmp(MPU6050 mpu1){ //this will get the DMP ready
majik 0:21019d94ad33 121 //initialize device
majik 0:21019d94ad33 122 mpu1.reset();
majik 0:21019d94ad33 123 DMP_DBG_MSG("\n\rInitializing I2C devices...")
majik 0:21019d94ad33 124 devStatus = mpu1.dmpInitialize();
majik 0:21019d94ad33 125 //insert gyro offsets here// Write a calibration algorithm in the future
majik 0:21019d94ad33 126 mpu1.setXGyroOffset(0); //800/25=32 //-31 is the largest offset available
majik 0:21019d94ad33 127 mpu1.setYGyroOffset(0);
majik 0:21019d94ad33 128 mpu1.setZGyroOffset(0);
majik 0:21019d94ad33 129 mpu1.setXAccelOffset(0); //the accel offsets don't do anything
majik 0:21019d94ad33 130 mpu1.setYAccelOffset(0);
majik 0:21019d94ad33 131 mpu1.setZAccelOffset(0);
majik 0:21019d94ad33 132
majik 0:21019d94ad33 133 mpu1.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
majik 0:21019d94ad33 134
majik 0:21019d94ad33 135 if(devStatus == 0){ //this means initialize was successful
majik 0:21019d94ad33 136 //turn on DMP
majik 0:21019d94ad33 137 DMP_DBG_MSG("\n\rEnabling DMP")
majik 0:21019d94ad33 138 mpu1.setDMPEnabled(true);
majik 0:21019d94ad33 139
majik 0:21019d94ad33 140 // enable interrupt detection
majik 0:21019d94ad33 141 DMP_DBG_MSG("Enabling interrupt detection (Arduino external interrupt 0)...\r\n")
majik 0:21019d94ad33 142 checkpin.rise(&dmpDataReady);
majik 0:21019d94ad33 143
majik 0:21019d94ad33 144 mpuIntStatus = mpu1.getIntStatus();
majik 0:21019d94ad33 145
majik 0:21019d94ad33 146 //set the DMP ready flag so main loop knows when it is ok to use
majik 0:21019d94ad33 147 DMP_DBG_MSG("DMP ready! Waiting for first interrupt")
majik 0:21019d94ad33 148 dmpReady = true;
majik 0:21019d94ad33 149
majik 0:21019d94ad33 150 //get expected DMP packet size
majik 0:21019d94ad33 151 packetSize = mpu1.dmpGetFIFOPacketSize();
majik 0:21019d94ad33 152 }else{
majik 0:21019d94ad33 153 DMP_DBG_MSG("\n\rDMP Initialization failed")
majik 0:21019d94ad33 154 DMP_DBG_MSG("\t%d",devStatus)
majik 0:21019d94ad33 155 Thread::wait(1000);
majik 0:21019d94ad33 156 }
majik 0:21019d94ad33 157 acc_offset[0]=0;
majik 0:21019d94ad33 158 acc_offset[1]=0;
majik 0:21019d94ad33 159 acc_offset[2]=0;
majik 0:21019d94ad33 160 mpu.setDLPFMode(MPU6050_DLPF_BW_42);
majik 0:21019d94ad33 161 }
majik 0:21019d94ad33 162 void start_dmp2(MPU6051 mpu1){ //this will get the DMP ready
majik 0:21019d94ad33 163 //initialize device
majik 0:21019d94ad33 164 mpu2.reset();
majik 0:21019d94ad33 165 DMP_DBG_MSG("\n\rInitializing I2C devices...")
majik 0:21019d94ad33 166 devStatus2 = mpu2.dmpInitialize();
majik 0:21019d94ad33 167 //insert gyro offsets here// Write a calibration algorithm in the future
majik 0:21019d94ad33 168 mpu2.setXGyroOffset(0); //800/25=32 //-31 is the largest offset available
majik 0:21019d94ad33 169 mpu2.setYGyroOffset(0);
majik 0:21019d94ad33 170 mpu2.setZGyroOffset(0);
majik 0:21019d94ad33 171 mpu2.setXAccelOffset(0); //the accel offsets don't do anything
majik 0:21019d94ad33 172 mpu2.setYAccelOffset(0);
majik 0:21019d94ad33 173 mpu2.setZAccelOffset(0);
majik 0:21019d94ad33 174
majik 0:21019d94ad33 175 mpu2.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
majik 0:21019d94ad33 176
majik 0:21019d94ad33 177 if(devStatus2 == 0){ //this means initialize was successful
majik 0:21019d94ad33 178 //turn on DMP
majik 0:21019d94ad33 179 DMP_DBG_MSG("\n\rEnabling DMP")
majik 0:21019d94ad33 180 mpu2.setDMPEnabled(true);
majik 0:21019d94ad33 181
majik 0:21019d94ad33 182 // enable interrupt detection
majik 0:21019d94ad33 183 DMP_DBG_MSG("Enabling interrupt detection (Arduino external interrupt 0)...\r\n")
majik 0:21019d94ad33 184 checkpin2.rise(&dmpDataReady2);
majik 0:21019d94ad33 185
majik 0:21019d94ad33 186 mpuIntStatus2 = mpu2.getIntStatus();
majik 0:21019d94ad33 187
majik 0:21019d94ad33 188 //set the DMP ready flag so main loop knows when it is ok to use
majik 0:21019d94ad33 189 DMP_DBG_MSG("DMP ready! Waiting for first interrupt")
majik 0:21019d94ad33 190 dmpReady2 = true;
majik 0:21019d94ad33 191
majik 0:21019d94ad33 192 //get expected DMP packet size
majik 0:21019d94ad33 193 packetSize2 = mpu2.dmpGetFIFOPacketSize();
majik 0:21019d94ad33 194 }else{
majik 0:21019d94ad33 195 DMP_DBG_MSG("\n\rDMP Initialization failed")
majik 0:21019d94ad33 196 DMP_DBG_MSG("\t%d",devStatus2)
majik 0:21019d94ad33 197 Thread::wait(1000);
majik 0:21019d94ad33 198 }
majik 0:21019d94ad33 199 acc_offset[0]=0;
majik 0:21019d94ad33 200 acc_offset[1]=0;
majik 0:21019d94ad33 201 acc_offset[2]=0;
majik 0:21019d94ad33 202 mpu2.setDLPFMode(MPU6050_DLPF_BW_42);
majik 0:21019d94ad33 203 }
majik 0:21019d94ad33 204
majik 0:21019d94ad33 205 void update_dmp(){
majik 0:21019d94ad33 206 if (!dmpReady) return;
majik 0:21019d94ad33 207 while (!mpuInterrupt && fifoCount < packetSize) {
majik 0:21019d94ad33 208 // other program behavior stuff here
majik 0:21019d94ad33 209 // if you are really paranoid you can frequently test in between other
majik 0:21019d94ad33 210 // stuff to see if mpuInterrupt is true, and if so, "break;" from the
majik 0:21019d94ad33 211 // while() loop to immediately process the MPU data
majik 0:21019d94ad33 212 // .
majik 0:21019d94ad33 213 }
majik 0:21019d94ad33 214 // reset interrupt flag and get INT_STATUS byte
majik 0:21019d94ad33 215 mpuInterrupt = false; //this resets the interrupt flag
majik 0:21019d94ad33 216 mpuIntStatus = mpu.getIntStatus();
majik 0:21019d94ad33 217
majik 0:21019d94ad33 218 //get current FIFO count;
majik 0:21019d94ad33 219 fifoCount = mpu.getFIFOCount();
majik 0:21019d94ad33 220
majik 0:21019d94ad33 221 //check for overflow (only happens if your code sucks)
majik 0:21019d94ad33 222 if((mpuIntStatus & 0x10) || fifoCount == 1024){
majik 0:21019d94ad33 223 //reset so we can continue cleanly
majik 0:21019d94ad33 224 mpu.resetFIFO();
majik 0:21019d94ad33 225 DMP_DBG_MSG("\n\rFIFO overflow")
majik 0:21019d94ad33 226 } else if (mpuIntStatus & 0x02){
majik 0:21019d94ad33 227 int16_t x,y,z;
majik 0:21019d94ad33 228 //wait for correct available data length (should be very short)
majik 0:21019d94ad33 229 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
majik 0:21019d94ad33 230
majik 0:21019d94ad33 231 //read a packet from FIFO
majik 0:21019d94ad33 232 mpu.getFIFOBytes(fifoBuffer, packetSize);
majik 0:21019d94ad33 233
majik 0:21019d94ad33 234 //track FIFO count here in the case there is > 1 packet available
majik 0:21019d94ad33 235 //(allows us to immediately read more w/o waiting for interrupt)
majik 0:21019d94ad33 236 fifoCount -= packetSize;
majik 0:21019d94ad33 237
majik 0:21019d94ad33 238 mpu.dmpGetQuaternion(&q,fifoBuffer);
majik 0:21019d94ad33 239 imu_data.quat[0]=q.w;
majik 0:21019d94ad33 240 imu_data.quat[1]=q.x;
majik 0:21019d94ad33 241 imu_data.quat[2]=q.y;
majik 0:21019d94ad33 242 imu_data.quat[3]=q.z;
majik 0:21019d94ad33 243 mpu.dmpGetGravity(&gravity, &q);
majik 0:21019d94ad33 244 mpu.dmpGetYawPitchRoll(ypr,&q,&gravity);
majik 0:21019d94ad33 245 imu_data.ypr[0] = ypr[0]*180/M_PI;//MARK - this saves the yaw data so everyone can access it
majik 0:21019d94ad33 246 imu_data.ypr[1] = ypr[1]*180/M_PI;
majik 0:21019d94ad33 247 imu_data.ypr[2] = ypr[2]*180/M_PI;
majik 0:21019d94ad33 248
majik 0:21019d94ad33 249 mpu.getAcceleration(&x,&y,&z);
majik 0:21019d94ad33 250 imu_data.acc[0] = (x-acc_offset[0]) *9.81/16384;
majik 0:21019d94ad33 251 imu_data.acc[1] = (y-acc_offset[1]) *9.81/16384;
majik 0:21019d94ad33 252 imu_data.acc[2] = (z-acc_offset[2]) *9.81/16384;
majik 0:21019d94ad33 253 #ifdef PRINT_GYR
majik 0:21019d94ad33 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:21019d94ad33 255 #endif
majik 0:21019d94ad33 256
majik 0:21019d94ad33 257 mpu.dmpGetAccel(&aa, fifoBuffer);
majik 0:21019d94ad33 258 mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
majik 0:21019d94ad33 259 #ifdef PRINT_ACC
majik 0:21019d94ad33 260 DMP_DBG_MSG("\n\rareal\t%d\t%d\t%d",aaReal.x,aaReal.y,aaReal.z)
majik 0:21019d94ad33 261 #endif
majik 0:21019d94ad33 262 //get rotations
majik 0:21019d94ad33 263 mpu.getMotion6(&x,&y,&z,&gx,&gy,&gz);
majik 0:21019d94ad33 264 float scale = 250/16384;
majik 0:21019d94ad33 265 imu_data.gyr[0] = gx*scale;
majik 0:21019d94ad33 266 imu_data.gyr[1] = gy*scale;
majik 0:21019d94ad33 267 imu_data.gyr[2] = gz*scale;
majik 0:21019d94ad33 268
majik 0:21019d94ad33 269 }
majik 0:21019d94ad33 270 }
majik 0:21019d94ad33 271 void update_dmp2(){
majik 0:21019d94ad33 272 if (!dmpReady2) return;
majik 0:21019d94ad33 273 while (!mpuInterrupt2 && fifoCount2 < packetSize2) {
majik 0:21019d94ad33 274 // other program behavior stuff here
majik 0:21019d94ad33 275 // if you are really paranoid you can frequently test in between other
majik 0:21019d94ad33 276 // stuff to see if mpuInterrupt is true, and if so, "break;" from the
majik 0:21019d94ad33 277 // while() loop to immediately process the MPU data
majik 0:21019d94ad33 278 // .
majik 0:21019d94ad33 279 }
majik 0:21019d94ad33 280 // reset interrupt flag and get INT_STATUS byte
majik 0:21019d94ad33 281 mpuInterrupt2 = false; //this resets the interrupt flag
majik 0:21019d94ad33 282 mpuIntStatus2 = mpu2.getIntStatus();
majik 0:21019d94ad33 283
majik 0:21019d94ad33 284 //get current FIFO count;
majik 0:21019d94ad33 285 fifoCount2 = mpu2.getFIFOCount();
majik 0:21019d94ad33 286
majik 0:21019d94ad33 287 //check for overflow (only happens if your code sucks)
majik 0:21019d94ad33 288 if((mpuIntStatus2 & 0x10) || fifoCount2 == 1024){
majik 0:21019d94ad33 289 //reset so we can continue cleanly
majik 0:21019d94ad33 290 mpu2.resetFIFO();
majik 0:21019d94ad33 291 DMP_DBG_MSG("\n\rFIFO overflow")
majik 0:21019d94ad33 292 } else if (mpuIntStatus2 & 0x02){
majik 0:21019d94ad33 293 int16_t x,y,z;
majik 0:21019d94ad33 294 //wait for correct available data length (should be very short)
majik 0:21019d94ad33 295 while (fifoCount2 < packetSize2) fifoCount2 = mpu2.getFIFOCount();
majik 0:21019d94ad33 296
majik 0:21019d94ad33 297 //read a packet from FIFO
majik 0:21019d94ad33 298 mpu2.getFIFOBytes(fifoBuffer2, packetSize2);
majik 0:21019d94ad33 299
majik 0:21019d94ad33 300 //track FIFO count here in the case there is > 1 packet available
majik 0:21019d94ad33 301 //(allows us to immediately read more w/o waiting for interrupt)
majik 0:21019d94ad33 302 fifoCount2 -= packetSize2;
majik 0:21019d94ad33 303
majik 0:21019d94ad33 304 mpu2.dmpGetQuaternion(&q,fifoBuffer2);
majik 0:21019d94ad33 305 imu2_data.quat[0]=q.w;
majik 0:21019d94ad33 306 imu2_data.quat[1]=q.x;
majik 0:21019d94ad33 307 imu2_data.quat[2]=q.y;
majik 0:21019d94ad33 308 imu2_data.quat[3]=q.z;
majik 0:21019d94ad33 309 mpu2.dmpGetGravity(&gravity, &q);
majik 0:21019d94ad33 310 mpu2.dmpGetYawPitchRoll(ypr,&q,&gravity);
majik 0:21019d94ad33 311 imu2_data.ypr[0] = ypr[0]*180/M_PI;//MARK - this saves the yaw data so everyone can access it
majik 0:21019d94ad33 312 imu2_data.ypr[1] = ypr[1]*180/M_PI;
majik 0:21019d94ad33 313 imu2_data.ypr[2] = ypr[2]*180/M_PI;
majik 0:21019d94ad33 314
majik 0:21019d94ad33 315 mpu2.getAcceleration(&x,&y,&z);
majik 0:21019d94ad33 316 imu2_data.acc[0] = (x-acc_offset[0]) *9.81/16384;
majik 0:21019d94ad33 317 imu2_data.acc[1] = (y-acc_offset[1]) *9.81/16384;
majik 0:21019d94ad33 318 imu2_data.acc[2] = (z-acc_offset[2]) *9.81/16384;
majik 0:21019d94ad33 319 #ifdef PRINT_GYR
majik 0:21019d94ad33 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:21019d94ad33 321 #endif
majik 0:21019d94ad33 322
majik 0:21019d94ad33 323 mpu2.dmpGetAccel(&aa, fifoBuffer2);
majik 0:21019d94ad33 324 mpu2.dmpGetLinearAccel(&aaReal, &aa, &gravity);
majik 0:21019d94ad33 325 #ifdef PRINT_ACC
majik 0:21019d94ad33 326 DMP_DBG_MSG("\n\rareal\t%d\t%d\t%d",aaReal.x,aaReal.y,aaReal.z)
majik 0:21019d94ad33 327 #endif
majik 0:21019d94ad33 328 //get rotations
majik 0:21019d94ad33 329 mpu2.getMotion6(&x,&y,&z,&gx,&gy,&gz);
majik 0:21019d94ad33 330 float scale = 250/16384;
majik 0:21019d94ad33 331 imu2_data.gyr[0] = gx*scale;
majik 0:21019d94ad33 332 imu2_data.gyr[1] = gy*scale;
majik 0:21019d94ad33 333 imu2_data.gyr[2] = gz*scale;
majik 0:21019d94ad33 334
majik 0:21019d94ad33 335 }
majik 0:21019d94ad33 336 }
majik 0:21019d94ad33 337 void update_acc()
majik 0:21019d94ad33 338 {
majik 0:21019d94ad33 339 int16_t x,y,z;
majik 0:21019d94ad33 340 mpu.getAcceleration(&x,&y,&z);
majik 0:21019d94ad33 341 imu_data.acc[0] = (x-acc_offset[0]) *9.81/16384;
majik 0:21019d94ad33 342 imu_data.acc[1] = (y-acc_offset[1]) *9.81/16384;
majik 0:21019d94ad33 343 imu_data.acc[2] = (z-acc_offset[2]) *9.81/16384;
majik 0:21019d94ad33 344 }
majik 0:21019d94ad33 345 void calibrate_1() //call this to calibrate the accelerometer
majik 0:21019d94ad33 346 { //in the future, modify the OFFSET REGISTERS to make the DMP converge faster
majik 0:21019d94ad33 347 //right now, I am only calculating the offset for the accelerometers
majik 0:21019d94ad33 348 int sum[3] = {0,0,0};
majik 0:21019d94ad33 349 int i;
majik 0:21019d94ad33 350 int16_t x,y,z;
majik 0:21019d94ad33 351
majik 0:21019d94ad33 352 int count=0;
majik 0:21019d94ad33 353 int t_now = t.read_ms();
majik 0:21019d94ad33 354 while((t.read_ms()-t_now) < 1000) //loop for one second
majik 0:21019d94ad33 355 {
majik 0:21019d94ad33 356 mpu.getAcceleration(&x,&y,&z);
majik 0:21019d94ad33 357 sum[0] += x;
majik 0:21019d94ad33 358 sum[1] += y;
majik 0:21019d94ad33 359 sum[2] += z;
majik 0:21019d94ad33 360 Thread::wait(5);
majik 0:21019d94ad33 361 count++;
majik 0:21019d94ad33 362 }
majik 0:21019d94ad33 363 for(i = 0; i<3; i++)
majik 0:21019d94ad33 364 acc_offset[i] = (sum[i]/count);
majik 0:21019d94ad33 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:21019d94ad33 366 }