Diff: RTOS-Threads/src/Task2_Master.cpp

Revision:

36:d95e3d6f2fc4

Parent:

34:228d87c45151

Child:

38:ef65533cca32

--- a/RTOS-Threads/src/Task2_Master.cpp	Tue May 13 02:44:10 2014 +0000
+++ b/RTOS-Threads/src/Task2_Master.cpp	Tue May 13 04:05:34 2014 +0000
@@ -2,18 +2,41 @@
  * Author:      Trung Tin Ian HUA
  * Date:        May 2014
  * Purpose:     Thread2M: Master PID control loop (attitude)
- * Settings:    100Hz
+ * Functions:   AHRSSample: Read MPU6050 DMP and calculate YPR
+ * Settings:    200Hz
  * Timing:      40us
  */
 #include "Task2_Slave.h"
 #include "setup.h"
 #include "PID.h"
 
+/* MPU6050 control/status variables: */
+uint8_t mpuIntStatus;       // holds actual interrupt status byte from MPU
+uint16_t fifoCount;         // count of all bytes currently in FIFO
+uint8_t fifoBuffer[64];     // FIFO storage buffer
+
+/* Orientation/motion variables: */
+Quaternion q;               // [w, x, y, z] quaternion container
+VectorFloat gravity;        // [x, y, z] gravity vector
+float ypr[3];               // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
+
+float altitude, temperature;
+
+#ifndef M_PI
+#define M_PI 3.1415
+#endif
+
+#ifdef ENABLE_COMPASS
+//int compassX, compassY, compassZ;
+double heading = 0;
+#endif
+
 /* YPR Adjust */
 volatile float adjust_attitude[3] = {0.0, 0.0, 0.0};
 
 
 
+
 // ===============================
 // === YPR SAMPLE & MASTER PID ===
 // ===============================
@@ -21,6 +44,8 @@
 void Task2_Master(void const *argument)
 {
 //Timer
+    AHRSSample();
+
     if (armed) {
         switch (mode) {
             case RATE:
@@ -29,8 +54,6 @@
             case ATTITUDE:
             default:
 //Timer
-                AHRSSample();
-
                 pitchPIDstable.setProcessValue((int) (ypr[1] - ypr_offset[1]));
                 rollPIDstable.setProcessValue((int) (ypr[2] - ypr_offset[2]));
 
@@ -49,6 +72,61 @@
     }
 }
 
+
+
+
 // ************************
 // *** Helper functions ***
 // ************************
+void AHRSSample(void)
+{
+//Timer
+    // reset interrupt flag and get INT_STATUS byte
+    mpuIntStatus = imu.getIntStatus();
+
+    // get current FIFO count
+    fifoCount = imu.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.debugSerial.printf("FIFO overflow!");
+
+        // 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 > 41) {
+            // read a packet from FIFO
+            imu.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)
+            fifoCount -= packetSize;
+        }
+
+        // display YPR angles in degrees
+        imu.dmpGetQuaternion(&q, fifoBuffer);
+        imu.dmpGetGravity(&gravity, &q);
+        imu.dmpGetYawPitchRoll(ypr, &q, &gravity);
+
+        ypr[0] = ypr[0] * 180/M_PI;
+        ypr[1] = ypr[1] * 180/M_PI;
+        ypr[2] = ypr[2] * 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
+}