Diff: DMP/DMP.cpp

Revision:

2:7b3822eacad8

Parent:

0:21019d94ad33

--- 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