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.

RTOS-Threads/src/Task4.cpp@19:bd88749c8db4, 2014-05-03 (annotated)

Committer:
pHysiX
Date:
Sat May 03 02:11:41 2014 +0000
Revision:
19:bd88749c8db4
Parent:
14:267368c83b6a
Child:
21:b642c18eccd1
Added constraints to RC Command and ESC output

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pHysiX 7:3d28cfc4901b 1 /* Update ESC (100Hz) */
pHysiX 7:3d28cfc4901b 2 /* 200Hz <= PWM frequency (400Hz) <= 400Hz */
pHysiX 2:ab967d7b4346 3
pHysiX 1:43f8ac7ca6d7 4 #include "tasks.h"
pHysiX 1:43f8ac7ca6d7 5 #include "setup.h"
pHysiX 1:43f8ac7ca6d7 6
pHysiX 2:ab967d7b4346 7 int ESCpower[4] = {0, 0, 0, 0};
pHysiX 10:ef5fe86f67fe 8 int stallESC = 0;
pHysiX 10:ef5fe86f67fe 9
pHysiX 14:267368c83b6a 10 bool armed = false;
pHysiX 2:ab967d7b4346 11
pHysiX 1:43f8ac7ca6d7 12 void Task4(void const *argurment)
pHysiX 1:43f8ac7ca6d7 13 {
pHysiX 14:267368c83b6a 14 if (armed) {
pHysiX 10:ef5fe86f67fe 15 if (counterESC) {
pHysiX 10:ef5fe86f67fe 16 /* [YAW PITCH ROLL THROTTLE AUX] */
pHysiX 10:ef5fe86f67fe 17 ESCpower[0] = (RCCommand[3]*9/10) + (pitch_adjust / 2) - (roll_adjust / 2) + (yaw_adjust / 2);
pHysiX 10:ef5fe86f67fe 18 ESCpower[1] = (RCCommand[3]*9/10) + (pitch_adjust / 2) + (roll_adjust / 2) - (yaw_adjust / 2);
pHysiX 10:ef5fe86f67fe 19 ESCpower[2] = (RCCommand[3]*9/10) - (pitch_adjust / 2) + (roll_adjust / 2) + (yaw_adjust / 2);
pHysiX 10:ef5fe86f67fe 20 ESCpower[3] = (RCCommand[3]*9/10) - (pitch_adjust / 2) - (roll_adjust / 2) - (yaw_adjust / 2);
pHysiX 19:bd88749c8db4 21
pHysiX 19:bd88749c8db4 22 for (int i = 0; i < 4; i++)
pHysiX 19:bd88749c8db4 23 ESC[i] = constrainESC(ESC[i]);
pHysiX 10:ef5fe86f67fe 24
pHysiX 19:bd88749c8db4 25 // if (ESC_check)
pHysiX 10:ef5fe86f67fe 26 //BT.printf("%4d %4d %4d %4d\n", ESCpower[0], ESCpower[1], ESCpower[2], ESCpower[3]);
pHysiX 10:ef5fe86f67fe 27
pHysiX 10:ef5fe86f67fe 28 for (int i = 0; i < 4; i++)
pHysiX 10:ef5fe86f67fe 29 ESC[i].pulsewidth_us(ESCpower[i]);
pHysiX 3:605fbcb54e75 30
pHysiX 10:ef5fe86f67fe 31 counterESC = false;
pHysiX 10:ef5fe86f67fe 32 } else {
pHysiX 10:ef5fe86f67fe 33 stallESC++;
pHysiX 10:ef5fe86f67fe 34
pHysiX 10:ef5fe86f67fe 35 if (stallESC > 1) {
pHysiX 10:ef5fe86f67fe 36 imu.debugSerial.printf("ESC NOT UPDATED FAST ENOUGH!\n");
pHysiX 10:ef5fe86f67fe 37 stallESC = 0;
pHysiX 10:ef5fe86f67fe 38 }
pHysiX 10:ef5fe86f67fe 39 }
pHysiX 10:ef5fe86f67fe 40 } else {
pHysiX 10:ef5fe86f67fe 41 for (int i = 0; i < 4; i++)
pHysiX 10:ef5fe86f67fe 42 ESC[i].pulsewidth_us(ESCpower[i]);
pHysiX 10:ef5fe86f67fe 43 }
pHysiX 3:605fbcb54e75 44
pHysiX 2:ab967d7b4346 45 //LED[4] = !LED[4];
pHysiX 19:bd88749c8db4 46 }
pHysiX 19:bd88749c8db4 47
pHysiX 19:bd88749c8db4 48 int constrainESC(int input)
pHysiX 19:bd88749c8db4 49 {
pHysiX 19:bd88749c8db4 50 if (input < 1000)
pHysiX 19:bd88749c8db4 51 return 1000;
pHysiX 19:bd88749c8db4 52 else if (input > 2000)
pHysiX 19:bd88749c8db4 53 return 2000;
pHysiX 19:bd88749c8db4 54 else
pHysiX 19:bd88749c8db4 55 return input;
pHysiX 19:bd88749c8db4 56 }