Diff: RTOS-Threads/src/Task2_Master.cpp

Revision:

50:8a0accb23007

Parent:

48:9dbdc4144f00

Child:

51:04c6af4319e1

--- a/RTOS-Threads/src/Task2_Master.cpp	Mon May 19 13:33:34 2014 +0000
+++ b/RTOS-Threads/src/Task2_Master.cpp	Mon May 19 14:16:47 2014 +0000
@@ -10,6 +10,8 @@
 #include "setup.h"
 #include "PID.h"
 
+Semaphore sem_Task2_Master(1);
+
 /* MPU6050 control/status variables: */
 uint8_t mpuIntStatus;       // holds actual interrupt status byte from MPU
 uint16_t fifoCount;         // count of all bytes currently in FIFO
@@ -19,9 +21,7 @@
 Quaternion q;               // [w, x, y, z] quaternion container
 VectorFloat gravity;        // [x, y, z] gravity vector
 float ypr_rad[3];           // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
-volatile float ypr[3];      
-
-float altitude, temperature;
+volatile float ypr[3];
 
 #ifndef M_PI
 #define M_PI 3.1415
@@ -41,33 +41,33 @@
 // ===============================
 // === YPR SAMPLE & MASTER PID ===
 // ===============================
-//Timer
 void Task2_Master(void const *argument)
 {
-//Timer
-    AHRSSample();
+    while(1) {
+        sem_Task2_Master.wait();
+        AHRSSample();
 
-    if (armed) {
-        switch (mode) {
-            case RATE:
-                break;
+        if (armed) {
+            switch (mode) {
+                case RATE:
+                    break;
 
-            case ATTITUDE:
-            default:
-//Timer
-                pitchPIDattitude.setProcessValue((int) (ypr[1] - ypr_offset[1]));
-                rollPIDattitude.setProcessValue((int) (ypr[2] - ypr_offset[2]));
+                case ATTITUDE:
+                default:
+                    pitchPIDattitude.setProcessValue((int) (ypr[1] - ypr_offset[1]));
+                    rollPIDattitude.setProcessValue((int) (ypr[2] - ypr_offset[2]));
+
+                    pitchPIDattitude.setSetPoint(inputYPR[1]);
+                    rollPIDattitude.setSetPoint(inputYPR[2]);
 
-                pitchPIDattitude.setSetPoint(inputYPR[1]);
-                rollPIDattitude.setSetPoint(inputYPR[2]);
+                    adjust_attitude[1] = pitchPIDattitude.compute();
+                    adjust_attitude[2] = rollPIDattitude.compute();
+                    adjust_attitude[2] *= -1;
 
-                adjust_attitude[1] = pitchPIDattitude.compute();
-                adjust_attitude[2] = rollPIDattitude.compute();
-                adjust_attitude[2] *= -1;
-//Timer
-                break;
+                    sem_Task2_Slave.release();
+                    break;
+            }
         }
-//Timer
     }
 }
 
@@ -77,6 +77,11 @@
 // ************************
 // *** Helper functions ***
 // ************************
+void Task2_Master_ISR(void const *argument)
+{
+    sem_Task2_Master.release();
+}
+
 void AHRSSample(void)
 {
 //Timer