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@21:b642c18eccd1, 2014-05-08 (annotated)

Committer:
pHysiX
Date:
Thu May 08 09:39:12 2014 +0000
Revision:
21:b642c18eccd1
Parent:
19:bd88749c8db4
Child:
22:ef8aa9728013
Primary PID disabled. Secondary (rate) PID enabled. P gain tuned

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 21:b642c18eccd1 11 bool ESC_check = false;
pHysiX 21:b642c18eccd1 12 bool calibration_mode = false;
pHysiX 2:ab967d7b4346 13
pHysiX 1:43f8ac7ca6d7 14 void Task4(void const *argurment)
pHysiX 1:43f8ac7ca6d7 15 {
pHysiX 14:267368c83b6a 16 if (armed) {
pHysiX 21:b642c18eccd1 17 if (calibration_mode) {
pHysiX 19:bd88749c8db4 18 for (int i = 0; i < 4; i++)
pHysiX 21:b642c18eccd1 19 ESC[i].pulsewidth_us(RCCommand[3]);
pHysiX 21:b642c18eccd1 20 } else if (RCCommand[3] > 1150) {
pHysiX 21:b642c18eccd1 21 if (counterESC) {
pHysiX 21:b642c18eccd1 22 /* [YAW PITCH ROLL THROTTLE AUX] */
pHysiX 21:b642c18eccd1 23
pHysiX 21:b642c18eccd1 24 for (int i = 0; i < 3; i++)
pHysiX 21:b642c18eccd1 25 adjust[i] /= 2.0;
pHysiX 21:b642c18eccd1 26
pHysiX 21:b642c18eccd1 27 ESCpower[0] = constrainESC((RCCommand[3]*9/10) + (adjust[1]) + (adjust[2]) - adjust[0]);
pHysiX 21:b642c18eccd1 28 ESCpower[1] = constrainESC((RCCommand[3]*9/10) + (adjust[1]) - (adjust[2]) + adjust[0]);
pHysiX 21:b642c18eccd1 29 ESCpower[2] = constrainESC((RCCommand[3]*9/10) - (adjust[1]) - (adjust[2]) - adjust[0]);
pHysiX 21:b642c18eccd1 30 ESCpower[3] = constrainESC((RCCommand[3]*9/10) - (adjust[1]) + (adjust[2]) + adjust[0]);
pHysiX 10:ef5fe86f67fe 31
pHysiX 21:b642c18eccd1 32 for (int i = 0; i < 4; i++)
pHysiX 21:b642c18eccd1 33 ESC[i].pulsewidth_us(ESCpower[i]);
pHysiX 10:ef5fe86f67fe 34
pHysiX 21:b642c18eccd1 35 counterESC = false;
pHysiX 21:b642c18eccd1 36 } else {
pHysiX 21:b642c18eccd1 37 stallESC++;
pHysiX 3:605fbcb54e75 38
pHysiX 21:b642c18eccd1 39 if (stallESC > 1) {
pHysiX 21:b642c18eccd1 40 imu.debugSerial.printf("ESC NOT UPDATED FAST ENOUGH!\n");
pHysiX 21:b642c18eccd1 41 stallESC = 0;
pHysiX 21:b642c18eccd1 42 }
pHysiX 21:b642c18eccd1 43 }
pHysiX 10:ef5fe86f67fe 44 } else {
pHysiX 21:b642c18eccd1 45 for (int i = 0; i < 4; i++) {
pHysiX 21:b642c18eccd1 46 ESCpower[i] = 980;
pHysiX 21:b642c18eccd1 47 ESC[i].pulsewidth_us(ESCpower[i]);
pHysiX 10:ef5fe86f67fe 48 }
pHysiX 10:ef5fe86f67fe 49 }
pHysiX 10:ef5fe86f67fe 50 } else {
pHysiX 21:b642c18eccd1 51 if (ESC_check) {
pHysiX 21:b642c18eccd1 52 BT.printf("Need to ARM to check ESC output!\n");
pHysiX 21:b642c18eccd1 53 ESC_check = false;
pHysiX 21:b642c18eccd1 54 }
pHysiX 21:b642c18eccd1 55
pHysiX 21:b642c18eccd1 56 for (int i = 0; i < 4; i++) {
pHysiX 21:b642c18eccd1 57 ESCpower[i] = 980;
pHysiX 10:ef5fe86f67fe 58 ESC[i].pulsewidth_us(ESCpower[i]);
pHysiX 21:b642c18eccd1 59 }
pHysiX 10:ef5fe86f67fe 60 }
pHysiX 3:605fbcb54e75 61
pHysiX 21:b642c18eccd1 62 if (ESC_check)
pHysiX 21:b642c18eccd1 63 BT.printf("%4d %4d %4d %4d\n", ESCpower[0], ESCpower[1], ESCpower[2], ESCpower[3]);
pHysiX 21:b642c18eccd1 64
pHysiX 2:ab967d7b4346 65 //LED[4] = !LED[4];
pHysiX 19:bd88749c8db4 66 }
pHysiX 19:bd88749c8db4 67
pHysiX 21:b642c18eccd1 68 int constrainESC(float input)
pHysiX 19:bd88749c8db4 69 {
pHysiX 21:b642c18eccd1 70 if (input < 1150.0)
pHysiX 21:b642c18eccd1 71 return 1150;
pHysiX 21:b642c18eccd1 72 else if (input > 2000.0)
pHysiX 19:bd88749c8db4 73 return 2000;
pHysiX 19:bd88749c8db4 74 else
pHysiX 21:b642c18eccd1 75 return (int) input;
pHysiX 19:bd88749c8db4 76 }