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

Revision:
10:ef5fe86f67fe
Parent:
7:3d28cfc4901b
Child:
14:267368c83b6a
--- a/RTOS-Threads/src/Task4.cpp	Wed Apr 30 13:49:12 2014 +0000
+++ b/RTOS-Threads/src/Task4.cpp	Thu May 01 23:50:21 2014 +0000
@@ -5,19 +5,37 @@
 #include "setup.h"
 
 int ESCpower[4] = {0, 0, 0, 0};
+int stallESC = 0;
+
 
 void Task4(void const *argurment)
 {
-    /* [YAW PITCH ROLL THROTTLE AUX] */
-    ESCpower[0] = (RCCommand[3]*9/10) + (pitch_adjust / 2) - (roll_adjust / 2) + (yaw_adjust / 2);
-    ESCpower[1] = (RCCommand[3]*9/10) + (pitch_adjust / 2) + (roll_adjust / 2) - (yaw_adjust / 2);
-    ESCpower[2] = (RCCommand[3]*9/10) - (pitch_adjust / 2) + (roll_adjust / 2) + (yaw_adjust / 2);
-    ESCpower[3] = (RCCommand[3]*9/10) - (pitch_adjust / 2) - (roll_adjust / 2) - (yaw_adjust / 2);
-    
-    //BT.printf("%4d %4d %4d %4d\n", ESCpower[0], ESCpower[1], ESCpower[2], ESCpower[3]);
+    if (RCCommand != 0) {
+        if (counterESC) {
+            /* [YAW PITCH ROLL THROTTLE AUX] */
+            ESCpower[0] = (RCCommand[3]*9/10) + (pitch_adjust / 2) - (roll_adjust / 2) + (yaw_adjust / 2);
+            ESCpower[1] = (RCCommand[3]*9/10) + (pitch_adjust / 2) + (roll_adjust / 2) - (yaw_adjust / 2);
+            ESCpower[2] = (RCCommand[3]*9/10) - (pitch_adjust / 2) + (roll_adjust / 2) + (yaw_adjust / 2);
+            ESCpower[3] = (RCCommand[3]*9/10) - (pitch_adjust / 2) - (roll_adjust / 2) - (yaw_adjust / 2);
+
+            //BT.printf("%4d %4d %4d %4d\n", ESCpower[0], ESCpower[1], ESCpower[2], ESCpower[3]);
+
+            for (int i = 0; i < 4; i++)
+                ESC[i].pulsewidth_us(ESCpower[i]);
 
-    for (int i = 0; i < 4; i++)
-        ESC[i].pulsewidth_us(ESCpower[i]);
+            counterESC = false;
+        } else {
+            stallESC++;
+
+            if (stallESC > 1) {
+                imu.debugSerial.printf("ESC NOT UPDATED FAST ENOUGH!\n");
+                stallESC = 0;
+            }
+        }
+    } else {
+        for (int i = 0; i < 4; i++)
+            ESC[i].pulsewidth_us(ESCpower[i]);
+    }
 
     //LED[4] = !LED[4];
-}
+}
\ No newline at end of file