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.cpp

Revision:
25:a7cfe421cb4a
Parent:
24:54a8cdf17378
diff -r 54a8cdf17378 -r a7cfe421cb4a RTOS-Threads/src/Task2.cpp
--- a/RTOS-Threads/src/Task2.cpp	Thu May 08 13:00:50 2014 +0000
+++ b/RTOS-Threads/src/Task2.cpp	Thu May 08 13:35:13 2014 +0000
@@ -3,7 +3,7 @@
  * Date:        May 2014
  * Purpose:     Thread2: Gyro sample and PID Control loop
  * Settings:    200Hz
- */ 
+ */
 
 #include "Task2.h"
 #include "setup.h"
@@ -29,32 +29,55 @@
         gyro[i] /= (float) 32.8;
 
     //if (counterTask1) {
-    pitchPIDstable.setSetPoint(inputYPR[1]);
-    rollPIDstable.setSetPoint(inputYPR[2]);
+
+    switch (mode) {
+        case RATE:
+            yawPIDrate.setSetPoint(inputYPR[0]);
+            pitchPIDrate.setSetPoint(inputYPR[1]);
+            rollPIDrate.setSetPoint(inputYPR[2]);
+
+            yawPIDrate.setProcessValue(gyro[2]);
+            pitchPIDrate.setProcessValue(gyro[1]);
+            rollPIDrate.setProcessValue(gyro[0]);
 
-    pitchPIDstable.setProcessValue((int) (ypr[1] - ypr_offset[1]));
-    rollPIDstable.setProcessValue((int) (ypr[2] - ypr_offset[2]));
+            adjust[0] = yawPIDrate.compute();
+            adjust[1] = pitchPIDrate.compute();
+            adjust[2] = rollPIDrate.compute();
+
+            counterTask1 = false;
+            counterESC = true;
 
-    adjust_stable[1] = pitchPIDstable.compute();
-    adjust_stable[2] = rollPIDstable.compute();
-    adjust_stable[2] *= -1;
+            break;
+
+        case STABLE:
+        default:
+            pitchPIDstable.setSetPoint(inputYPR[1]);
+            rollPIDstable.setSetPoint(inputYPR[2]);
 
-    yawPIDrate.setSetPoint(inputYPR[0]);
-    //pitchPIDrate.setSetPoint(inputYPR[1]);
-    //rollPIDrate.setSetPoint(inputYPR[2]);
-    pitchPIDrate.setSetPoint(adjust_stable[1]);
-    rollPIDrate.setSetPoint(adjust_stable[2]);
+            pitchPIDstable.setProcessValue((int) (ypr[1] - ypr_offset[1]));
+            rollPIDstable.setProcessValue((int) (ypr[2] - ypr_offset[2]));
+
+            adjust_stable[1] = pitchPIDstable.compute();
+            adjust_stable[2] = rollPIDstable.compute();
+            adjust_stable[2] *= -1;
+
+            yawPIDrate.setSetPoint(inputYPR[0]);
+            pitchPIDrate.setSetPoint(adjust_stable[1]);
+            rollPIDrate.setSetPoint(adjust_stable[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();
 
-    counterTask1 = false;
-    counterESC = true;
+            counterTask1 = false;
+            counterESC = true;
+            
+            break;
+    }
 
     if (adjust_check)
         BT.printf("%3.4f %3.4f %3.4f\n", adjust[0], adjust[1], adjust[2]);