Diff: RTOS-Threads/src/Task3.cpp

Revision:

30:d9b988f8d84f

Parent:

27:18b6580eb0b1

Child:

31:3dde2201e54d

diff -r 6c03b30b59ec -r d9b988f8d84f RTOS-Threads/src/Task3.cpp
--- a/RTOS-Threads/src/Task3.cpp	Sat May 10 04:06:53 2014 +0000
+++ b/RTOS-Threads/src/Task3.cpp	Mon May 12 04:43:38 2014 +0000
@@ -4,7 +4,6 @@
  * Purpose:     Thread3: RC & BT Command, and Telemetry
  * Settings:    50Hz
  */
-
 #define STICK_GAIN 2
 #define STICK_GAIN_YAW 16
 
@@ -12,7 +11,8 @@
 #include "setup.h"
 #include "PwmIn.h"
 
-float ypr_offset[3];
+PwmIn rxModule[] = {p14, p15, p16, p17, p18};
+AnalogIn voltageSense(p20);
 
 bool box_demo = false;
 bool rc_out = false;
@@ -20,20 +20,25 @@
 bool command_check = false;
 bool adjust_check = false;
 
-PwmIn rxModule[] = {p14, p15, p16, p17, p18};
-AnalogIn voltageSense(p20);
-
-float vIn = 0.0;
-
 /* [YAW PITCH ROLL THROTTLE AUX] */
 int RCCommand[5] = {0, 0, 0, 0, 0};
 /* Decoded input: [YAW PITCH ROLL] */
 int inputYPR[3];
+float ypr_offset[3];
+
+float vIn = 0.0;
 
 FLIGHT_MODE mode = ATTITUDE;
 
+#ifdef TIME_TASK3
+Timer _t3;
+#endif
 void Task3(void const *argument)
 {
+#ifdef TIME_TASK3
+    _t3.reset();
+    _t3.start();
+#endif
     if (BT.readable()) {
         char data = BT.getc();
 
@@ -333,17 +338,24 @@
         }
     }
 
+    /* Receiver decoder: */
     RCCommand[2] = rxModule[0].pulsewidth(); // Roll
     RCCommand[1] = rxModule[1].pulsewidth(); // Pitch
     RCCommand[3] = rxModule[2].pulsewidth(); // Throttle
     RCCommand[0] = rxModule[3].pulsewidth(); // Yaw
     RCCommand[4] = rxModule[4].pulsewidth(); // AUX
+    
+    /* Throttle decoder: */
+    RCCommand[3] = RCCommand[3] * 9/10;
+    //RCCommand[3] = (RCCommand[3] + (RCCommand[3]<<3))/10;
 
+    /* Mode switching: */
     if (RCCommand[4] > 1500)
         mode = RATE;
     else
         mode = ATTITUDE;
 
+    /* Command decoder: */
     inputYPR[0] = (RCCommand[0]-1500)*9/100*STICK_GAIN_YAW;
     inputYPR[1] = (RCCommand[1]-1500)*-1*9/100*STICK_GAIN;
     switch (mode) {
@@ -372,6 +384,11 @@
         BT.printf("%5d %5d %5d %5d %5d\n", RCCommand[0], RCCommand[1], RCCommand[2], RCCommand[3], RCCommand[4]);
     else if (command_check)
         BT.printf("%3d %3d %3d\n", inputYPR[0], inputYPR[1], inputYPR[2]);
+
+#ifdef TIME_TASK3
+    _t3.stop();
+    BT.printf("%d\n", _t3.read_us());
+#endif
 }
 
 int constrainRCCommand(int input)