This is for our FYDP project. 2 MPU6050s are used
Diff: DMP/DMP.cpp
- Revision:
- 2:7b3822eacad8
- Parent:
- 0:21019d94ad33
diff -r 815f16490da8 -r 7b3822eacad8 DMP/DMP.cpp --- a/DMP/DMP.cpp Sat Mar 21 21:54:52 2015 +0000 +++ b/DMP/DMP.cpp Sat Mar 21 22:44:36 2015 +0000 @@ -52,12 +52,16 @@ // orientation/motion vars Quaternion q; // [w,x,y,z] quaternion container +Quaternion q2; // [w,x,y,z] quaternion container VectorInt16 aa; // [x,y,z] accel sensor measurements VectorInt16 aaReal; // [x,y,z] gravity-free accel sensor measurements VectorInt16 aaWorld; // [x,y,z] world-frame accel sensor measurements VectorFloat gravity; // [x,y,z] gravity vector +VectorFloat gravity2; // [x,y,z] gravity vector + float euler[3]; // [psi,theta,phi] Euler angle container float ypr[3]; // [yaw,pitch,roll] yaw/pitch/roll container [rad]. Multiply by 180, divide by PI for [deg] +float ypr2[3]; InterruptIn checkpin(PTD7); //interrupt InterruptIn checkpin2(PTD5); //PTD5 //*** Interrupt Detection Routine ***// @@ -301,27 +305,27 @@ //(allows us to immediately read more w/o waiting for interrupt) fifoCount2 -= packetSize2; - mpu2.dmpGetQuaternion(&q,fifoBuffer2); - imu2_data.quat[0]=q.w; - imu2_data.quat[1]=q.x; - imu2_data.quat[2]=q.y; - imu2_data.quat[3]=q.z; - mpu2.dmpGetGravity(&gravity, &q); - mpu2.dmpGetYawPitchRoll(ypr,&q,&gravity); - imu2_data.ypr[0] = ypr[0]*180/M_PI;//MARK - this saves the yaw data so everyone can access it - imu2_data.ypr[1] = ypr[1]*180/M_PI; - imu2_data.ypr[2] = ypr[2]*180/M_PI; + mpu2.dmpGetQuaternion(&q2,fifoBuffer2); + imu2_data.quat[0]=q2.w; + imu2_data.quat[1]=q2.x; + imu2_data.quat[2]=q2.y; + imu2_data.quat[3]=q2.z; + mpu2.dmpGetGravity(&gravity2, &q2); + mpu2.dmpGetYawPitchRoll(ypr2,&q2,&gravity2); + imu2_data.ypr[0] = ypr2[0]*180/M_PI;//MARK - this saves the yaw data so everyone can access it + imu2_data.ypr[1] = ypr2[1]*180/M_PI; + imu2_data.ypr[2] = ypr2[2]*180/M_PI; mpu2.getAcceleration(&x,&y,&z); imu2_data.acc[0] = (x-acc_offset[0]) *9.81/16384; imu2_data.acc[1] = (y-acc_offset[1]) *9.81/16384; imu2_data.acc[2] = (z-acc_offset[2]) *9.81/16384; #ifdef PRINT_GYR - DMP_DBG_MSG("\n\rypr\t %f\t %f\t %f",ypr[0]*180/PI,ypr[1]*180/PI,ypr[2]*180/PI) + DMP_DBG_MSG("\n\rypr\t %f\t %f\t %f",ypr2[0]*180/PI,ypr2[1]*180/PI,ypr2[2]*180/PI) #endif mpu2.dmpGetAccel(&aa, fifoBuffer2); - mpu2.dmpGetLinearAccel(&aaReal, &aa, &gravity); + mpu2.dmpGetLinearAccel(&aaReal, &aa, &gravity2); #ifdef PRINT_ACC DMP_DBG_MSG("\n\rareal\t%d\t%d\t%d",aaReal.x,aaReal.y,aaReal.z) #endif