Ian Hua / Quadcopter-mbedRTOS

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Diff: RTOS-Threads/src/Task2_Slave.cpp

Revision:
50:8a0accb23007
Parent:
48:9dbdc4144f00
Child:
51:04c6af4319e1
--- a/RTOS-Threads/src/Task2_Slave.cpp	Mon May 19 13:33:34 2014 +0000
+++ b/RTOS-Threads/src/Task2_Slave.cpp	Mon May 19 14:16:47 2014 +0000
@@ -10,6 +10,8 @@
 #include "setup.h"
 #include "PID.h"
 
+Semaphore sem_Task2_Slave(1);
+
 /* YPR Adjust */
 volatile float adjust[3] = {0.0, 0.0, 0.0};
 
@@ -23,51 +25,50 @@
 // ===============================
 // === GYRO SAMPLE & SLAVE PID ===
 // ===============================
-//Timer
 void Task2_Slave(void const *argument)
 {
-//Timer
-    gyroSample();
+    while (1) {
+        sem_Task2_Slave.wait();
+        gyroSample();
 
-//Timer
-    if (armed) {
-        yawPIDrate.setSetPoint(inputYPR[0]);
+        if (armed) {
+            yawPIDrate.setSetPoint(inputYPR[0]);
 
-        switch (mode) {
-            case RATE:
-                pitchPIDrate.setSetPoint(inputYPR[1]);
-                rollPIDrate.setSetPoint(inputYPR[2]);
+            switch (mode) {
+                case RATE:
+                    pitchPIDrate.setSetPoint(inputYPR[1]);
+                    rollPIDrate.setSetPoint(inputYPR[2]);
 
-                yawPIDrate.setProcessValue(gyro[2]);
-                pitchPIDrate.setProcessValue(gyro[1]);
-                rollPIDrate.setProcessValue(gyro[0]);
+                    yawPIDrate.setProcessValue(gyro[2]);
+                    pitchPIDrate.setProcessValue(gyro[1]);
+                    rollPIDrate.setProcessValue(gyro[0]);
 
-                adjust[0] = yawPIDrate.compute();
-                adjust[1] = pitchPIDrate.compute();
-                adjust[2] = rollPIDrate.compute();
+                    adjust[0] = yawPIDrate.compute();
+                    adjust[1] = pitchPIDrate.compute();
+                    adjust[2] = rollPIDrate.compute();
 
-                counterESC = true;
-                break;
+                    counterESC = true;
+                    break;
 
-            case ATTITUDE:
-            default:
-                pitchPIDrate.setSetPoint(adjust_attitude[1]);
-                rollPIDrate.setSetPoint(adjust_attitude[2]);
+                case ATTITUDE:
+                default:
+                    pitchPIDrate.setSetPoint(adjust_attitude[1]);
+                    rollPIDrate.setSetPoint(adjust_attitude[2]);
 
-                yawPIDrate.setProcessValue(gyro[2]);
-                pitchPIDrate.setProcessValue(gyro[1]);
-                rollPIDrate.setProcessValue(gyro[0]);
+                    yawPIDrate.setProcessValue(gyro[2]);
+                    pitchPIDrate.setProcessValue(gyro[1]);
+                    rollPIDrate.setProcessValue(gyro[0]);
 
-                adjust[0] = yawPIDrate.compute();
-                adjust[1] = pitchPIDrate.compute();
-                adjust[2] = rollPIDrate.compute();
+                    adjust[0] = yawPIDrate.compute();
+                    adjust[1] = pitchPIDrate.compute();
+                    adjust[2] = rollPIDrate.compute();
 
-                counterESC = true;
-                break;
+                    counterESC = true;
+                    break;
+            }
+            //sem_Task4.release();
         }
-//Timer
     }
-//Timer
 }
 
 
@@ -76,6 +77,11 @@
 // ************************
 // *** Helper functions ***
 // ************************
+void Task2_Slave_ISR(void const *argument)
+{
+    sem_Task2_Slave.release();
+}
+
 void gyroSample(void)
 {
     imu_nb.getRotation(&gx, &gy, &gz);