Diff: RTOS-Threads/src/Task2_Master.cpp

Revision:

27:18b6580eb0b1

Parent:

26:4a3323ee36d5

Child:

28:aa72bd4ff103

--- a/RTOS-Threads/src/Task2_Master.cpp	Sat May 10 01:04:17 2014 +0000
+++ b/RTOS-Threads/src/Task2_Master.cpp	Sat May 10 01:39:02 2014 +0000
@@ -0,0 +1,42 @@
+/* File:        Task2_Master.cpp
+ * Author:      Trung Tin Ian HUA
+ * Date:        May 2014
+ * Purpose:     Thread2: Gyro sample and PID Control loop
+ * Settings:    200Hz
+ */
+
+#include "Task2_Slave.h"
+#include "setup.h"
+#include "PID.h"
+
+/* YPR Adjust */
+
+volatile float adjust_attitude[3] = {0.0, 0.0, 0.0};
+
+//bool counterTask2Master = false;
+
+void Task2_Master(void const *argument)
+{
+    switch (mode) {
+        case RATE:
+            break;
+            
+        case ATTITUDE:
+        default:
+            if (counterTask1) {
+                pitchPIDstable.setSetPoint(inputYPR[1]);
+                rollPIDstable.setSetPoint(inputYPR[2]);
+
+                pitchPIDstable.setProcessValue((int) (ypr[1] - ypr_offset[1]));
+                rollPIDstable.setProcessValue((int) (ypr[2] - ypr_offset[2]));
+
+                adjust_attitude[1] = pitchPIDstable.compute();
+                adjust_attitude[2] = rollPIDstable.compute();
+                adjust_attitude[2] *= -1;
+
+                counterTask1 = false;
+                //counterTask2Master = true;
+            }
+            break;
+    }
+}