Diff: RTOS-Threads/src/Task2_Master.cpp

Revision:

30:d9b988f8d84f

Parent:

28:aa72bd4ff103

Child:

31:3dde2201e54d

--- a/RTOS-Threads/src/Task2_Master.cpp	Sat May 10 04:06:53 2014 +0000
+++ b/RTOS-Threads/src/Task2_Master.cpp	Mon May 12 04:43:38 2014 +0000
@@ -2,9 +2,9 @@
  * Author:      Trung Tin Ian HUA
  * Date:        May 2014
  * Purpose:     Thread2: Gyro sample and PID Control loop
- * Settings:    200Hz
+ * Settings:    100Hz
+ * Timing:      40us
  */
-
 #include "Task2_Slave.h"
 #include "setup.h"
 #include "PID.h"
@@ -15,28 +15,47 @@
 
 //bool counterTask2Master = false;
 
+#ifdef TIME_TASK2M
+Timer _t2m;
+#endif
 void Task2_Master(void const *argument)
 {
-    switch (mode) {
-        case RATE:
-            break;
+#ifndef TIME_TASK2M
+    if (armed) {
+#endif
+        switch (mode) {
+            case RATE:
+                break;
 
-        case ATTITUDE:
-        default:
-            if (counterTask1) {
-                pitchPIDstable.setProcessValue((int) (ypr[1] - ypr_offset[1]));
-                rollPIDstable.setProcessValue((int) (ypr[2] - ypr_offset[2]));
-            }
+            case ATTITUDE:
+            default:
+#ifdef TIME_TASK2M
+                _t2m.reset();
+                _t2m.start();
+#endif
+                Task1(NULL);
+
+                if (counterTask1) {
+                    pitchPIDstable.setProcessValue((int) (ypr[1] - ypr_offset[1]));
+                    rollPIDstable.setProcessValue((int) (ypr[2] - ypr_offset[2]));
+                }
 
-            pitchPIDstable.setSetPoint(inputYPR[1]);
-            rollPIDstable.setSetPoint(inputYPR[2]);
+                pitchPIDstable.setSetPoint(inputYPR[1]);
+                rollPIDstable.setSetPoint(inputYPR[2]);
+
+                adjust_attitude[1] = pitchPIDstable.compute();
+                adjust_attitude[2] = rollPIDstable.compute();
+                adjust_attitude[2] *= -1;
 
-            adjust_attitude[1] = pitchPIDstable.compute();
-            adjust_attitude[2] = rollPIDstable.compute();
-            adjust_attitude[2] *= -1;
-
-            counterTask1 = false;
-            //counterTask2Master = true;
-            break;
+                counterTask1 = false;
+                //counterTask2Master = true;
+#ifdef TIME_TASK2M
+                _t2m.stop();
+                BT.printf("%d\n", _t2m.read_us());
+#endif
+                break;
+        }
+#ifndef TIME_TASK2M
     }
+#endif
 }