Diff: RTOS-Threads/src/Task2_Slave.cpp

Revision:

30:d9b988f8d84f

Parent:

27:18b6580eb0b1

Child:

31:3dde2201e54d

--- a/RTOS-Threads/src/Task2_Slave.cpp	Sat May 10 04:06:53 2014 +0000
+++ b/RTOS-Threads/src/Task2_Slave.cpp	Mon May 12 04:43:38 2014 +0000
@@ -3,8 +3,8 @@
  * Date:        May 2014
  * Purpose:     Thread2: Gyro sample and PID Control loop
  * Settings:    200Hz
+ * Timing:      290us
  */
-
 #include "Task2_Slave.h"
 #include "setup.h"
 #include "PID.h"
@@ -17,8 +17,15 @@
 
 bool counterESC = false;
 
+#ifdef TIME_TASK2S
+Timer _t2s;
+#endif
 void Task2_Slave(void const *argument)
 {
+#ifdef TIME_TASK2S
+    _t2s.reset();
+    _t2s.start();
+#endif
     imu.getRotation(&gx, &gy, &gz);
     gyro[0] = gx + 60;
     gyro[1] = gy - 15;
@@ -27,44 +34,55 @@
     for (int i = 0; i < 3; i++)
         gyro[i] /= (float) 32.8;
 
-    yawPIDrate.setSetPoint(inputYPR[0]);
-
-    switch (mode) {
-        case RATE:
-            pitchPIDrate.setSetPoint(inputYPR[1]);
-            rollPIDrate.setSetPoint(inputYPR[2]);
+#ifndef TIME_TASK2S
+    if (armed) {
+#endif
+        yawPIDrate.setSetPoint(inputYPR[0]);
 
-            yawPIDrate.setProcessValue(gyro[2]);
-            pitchPIDrate.setProcessValue(gyro[1]);
-            rollPIDrate.setProcessValue(gyro[0]);
+        switch (mode) {
+            case RATE:
+                pitchPIDrate.setSetPoint(inputYPR[1]);
+                rollPIDrate.setSetPoint(inputYPR[2]);
 
-            adjust[0] = yawPIDrate.compute();
-            adjust[1] = pitchPIDrate.compute();
-            adjust[2] = rollPIDrate.compute();
+                yawPIDrate.setProcessValue(gyro[2]);
+                pitchPIDrate.setProcessValue(gyro[1]);
+                rollPIDrate.setProcessValue(gyro[0]);
+
+                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;
+        }
+#ifndef TIME_TASK2S
     }
+#endif
 
     if (adjust_check)
         BT.printf("%3.4f %3.4f %3.4f\n", adjust[0], adjust[1], adjust[2]);
 
     if (gyro_out)
         BT.printf("%3d %3d %3d\n", (int) gyro[0], (int) gyro[1], (int) gyro[2]);
+
+#ifdef TIME_TASK2S
+    _t2s.stop();
+    BT.printf("%d\n", _t2s.read_us());
+#endif
 }