Diff: RTOS-Threads/src/Task2_Master.cpp

Revision:

48:9dbdc4144f00

Parent:

39:02782ad251db

Child:

50:8a0accb23007

--- a/RTOS-Threads/src/Task2_Master.cpp	Sat May 17 11:57:09 2014 +0000
+++ b/RTOS-Threads/src/Task2_Master.cpp	Mon May 19 13:21:17 2014 +0000
@@ -64,8 +64,6 @@
                 adjust_attitude[1] = pitchPIDattitude.compute();
                 adjust_attitude[2] = rollPIDattitude.compute();
                 adjust_attitude[2] *= -1;
-
-                //counterTask2Master = true;
 //Timer
                 break;
         }
@@ -83,28 +81,28 @@
 {
 //Timer
     // reset interrupt flag and get INT_STATUS byte
-    mpuIntStatus = imu.getIntStatus();
+    mpuIntStatus = imu_nb.getIntStatus();
 
     // get current FIFO count
-    fifoCount = imu.getFIFOCount();
+    fifoCount = imu_nb.getFIFOCount();
     //imu.debugSerial.printf("FIFO Count: %d\n", fifoCount);
 
     // check for overflow
     // Only keep a max of 2 packets in buffer.
     if ((mpuIntStatus & 0x10) || fifoCount > 84) {
         // reset so we can continue cleanly
-        imu.resetFIFO();
+        imu_nb.resetFIFO();
         //imu.debugSerial.printf("FIFO overflow!");
         //BT.printf("FIFO overflow!\n");
 
         // otherwise, check for DMP data ready interrupt (this should happen frequently)
     } else if (mpuIntStatus & 0x02) {
         // wait for correct available data length, should be a VERY short wait
-        while (fifoCount < packetSize) fifoCount = imu.getFIFOCount();
+        while (fifoCount < packetSize) fifoCount = imu_nb.getFIFOCount();
 
         while (fifoCount > 41) {
             // read a packet from FIFO
-            imu.getFIFOBytes(fifoBuffer, packetSize);
+            imu_nb.getFIFOBytes(fifoBuffer, packetSize);
 
             // track FIFO count here in case there is > 1 packet available
             // (this lets us immediately read more without waiting for an interrupt)
@@ -112,22 +110,12 @@
         }
 
         // display YPR angles in degrees
-        imu.dmpGetQuaternion(&q, fifoBuffer);
-        imu.dmpGetGravity(&gravity, &q);
-        imu.dmpGetYawPitchRoll(ypr_rad, &q, &gravity);
+        imu_nb.dmpGetQuaternion(&q, fifoBuffer);
+        imu_nb.dmpGetGravity(&gravity, &q);
+        imu_nb.dmpGetYawPitchRoll(ypr_rad, &q, &gravity);
 
         for (int i = 0; i < 3; i++)
             ypr[i] = ypr_rad[i] * 180/M_PI;
-
-        /*
-        if (compass.getDataReady()) {
-            // compass.getValues(&compass_x, &compass_y, &compass_z);
-            heading = compass.getHeadingXY() * 180/M_PI;
-        }
-
-        ypr[0] *= 0.98;
-        ypr[0] += 0.02*heading;
-        */
     }
 //Timer
 }