Diff: RTOS-Threads/src/Task1.cpp

Revision:

36:d95e3d6f2fc4

Parent:

34:228d87c45151

Child:

37:29feef05d848

--- a/RTOS-Threads/src/Task1.cpp	Tue May 13 02:44:10 2014 +0000
+++ b/RTOS-Threads/src/Task1.cpp	Tue May 13 04:05:34 2014 +0000
@@ -1,109 +1,56 @@
 /* File:        Task1.cpp
  * Author:      Trung Tin Ian HUA
  * Date:        May 2014
- * Functions:   Thread1: Service YPR telemetry output
- * Functions:   AHRSSample: Read MPU6050 DMP and calculate YPR
+ * Functions:   Thread1: Telemetry output
  * Settings:    100Hz
- * Timing:      1440us
+ * Timing:      //1440us
  */
 #include "Task1.h"
 #include "setup.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 TELEMETRY ===
-// =====================
-void Task1(void const *argument)
-{
-    switch (box_demo) {
-        case true:
-            AHRSSample();
-            if (box_demo)
-                BT.printf("\nY%3.2f\nP%3.2f\nR%3.2f\n", ypr[0] - ypr_offset[0],
-                          ypr[1] - ypr_offset[1], ypr[2] - ypr_offset[2]);
-            break;
-    }
-}
-//Timer
+bool box_demo = false;
+bool rc_out = false;
+bool gyro_out = false;
+bool command_check = false;
+bool adjust_check = false;
 
 
 
 
-// ************************
-// *** Helper functions ***
-// ************************
-void AHRSSample(void)
+// =================
+// === TELEMETRY ===
+// =================
+void Task1(void const *argument)
 {
-//Timer
-    // reset interrupt flag and get INT_STATUS byte
-    mpuIntStatus = imu.getIntStatus();
+    switch (box_demo) {
+        case true:
+            BT.printf("\nY%3.2f\nP%3.2f\nR%3.2f\n", ypr[0] - ypr_offset[0], ypr[1] - ypr_offset[1], ypr[2] - ypr_offset[2]);
+            BT.printf("\nA%3.2f\nT%2.2f\n", altimeter.Altitude_m(), altimeter.Temp_C());
 
-    // 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();
+            BT.printf("\nV%2.2f\n", voltageSense*VOLTAGE_SCALE);
+            break;
+        case false:
+            AHRSSample();
+            break;
+        default:
+            break;
+    }
 
-        while (fifoCount > 41) {
-            // read a packet from FIFO
-            imu.getFIFOBytes(fifoBuffer, packetSize);
+    if (gyro_out)
+        BT.printf("%3d %3d %3d\n", (int) gyro[0], (int) gyro[1], (int) gyro[2]);
 
-            // track FIFO count here in case there is > 1 packet available
-            // (this lets us immediately read more without waiting for an interrupt)
-            fifoCount -= packetSize;
-        }
+    else if (rc_out)
+        BT.printf("%5d %5d %5d %5d %5d\n", RCCommand[0], RCCommand[1], RCCommand[2], RCCommand[3], RCCommand[4]);
 
-        // display YPR angles in degrees
-        imu.dmpGetQuaternion(&q, fifoBuffer);
-        imu.dmpGetGravity(&gravity, &q);
-        imu.dmpGetYawPitchRoll(ypr, &q, &gravity);
+    else if (command_check)
+        BT.printf("%3d %3d %3d\n", inputYPR[0], inputYPR[1], inputYPR[2]);
 
-        ypr[0] = ypr[0] * 180/M_PI;
-        ypr[1] = ypr[1] * 180/M_PI;
-        ypr[2] = ypr[2] * 180/M_PI;
+    else if (adjust_check)
+        BT.printf("%3.2f %3.2f %3.2f\n", adjust[0], adjust[1], adjust[2]);
 
-        /*
-        if (compass.getDataReady()) {
-            // compass.getValues(&compass_x, &compass_y, &compass_z);
-            heading = compass.getHeadingXY() * 180/M_PI;
-        }
+    else if (ESC_check)
+        BT.printf("%4d %4d %4d %4d\n", ESCpower[0], ESCpower[1], ESCpower[2], ESCpower[3]);
 
-        ypr[0] *= 0.98;
-        ypr[0] += 0.02*heading;
-        */
-    }
+    else {}
+}
 //Timer
-}