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@30:d9b988f8d84f, 2014-05-12 (annotated)

Committer:
pHysiX
Date:
Mon May 12 04:43:38 2014 +0000
Revision:
30:d9b988f8d84f
Parent:
27:18b6580eb0b1
Child:
31:3dde2201e54d
WIP

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pHysiX 22:ef8aa9728013 1 /* File: Task4.h
pHysiX 22:ef8aa9728013 2 * Author: Trung Tin Ian HUA
pHysiX 22:ef8aa9728013 3 * Date: May 2014
pHysiX 22:ef8aa9728013 4 * Purpose: Thread4: ESC pulsewidth update. Note this is INDEPENDENT of the pulse frequency.
pHysiX 22:ef8aa9728013 5 * Settings: 200Hz
pHysiX 22:ef8aa9728013 6 * 200Hz <= PWM frequency <= 400Hz
pHysiX 30:d9b988f8d84f 7 * Refer to tasks.h to change PWM frequency
pHysiX 30:d9b988f8d84f 8 * Timing: 1500us
pHysiX 30:d9b988f8d84f 9 */
pHysiX 1:43f8ac7ca6d7 10 #include "tasks.h"
pHysiX 1:43f8ac7ca6d7 11 #include "setup.h"
pHysiX 1:43f8ac7ca6d7 12
pHysiX 2:ab967d7b4346 13 int ESCpower[4] = {0, 0, 0, 0};
pHysiX 10:ef5fe86f67fe 14 int stallESC = 0;
pHysiX 10:ef5fe86f67fe 15
pHysiX 14:267368c83b6a 16 bool armed = false;
pHysiX 21:b642c18eccd1 17 bool ESC_check = false;
pHysiX 21:b642c18eccd1 18 bool calibration_mode = false;
pHysiX 2:ab967d7b4346 19
pHysiX 30:d9b988f8d84f 20 #ifdef TIME_TASK4
pHysiX 30:d9b988f8d84f 21 Timer _t4;
pHysiX 30:d9b988f8d84f 22 #endif
pHysiX 1:43f8ac7ca6d7 23 void Task4(void const *argurment)
pHysiX 1:43f8ac7ca6d7 24 {
pHysiX 30:d9b988f8d84f 25 #ifdef TIME_TASK4
pHysiX 30:d9b988f8d84f 26 _t4.reset();
pHysiX 30:d9b988f8d84f 27 _t4.start();
pHysiX 30:d9b988f8d84f 28 #endif
pHysiX 14:267368c83b6a 29 if (armed) {
pHysiX 21:b642c18eccd1 30 if (calibration_mode) {
pHysiX 19:bd88749c8db4 31 for (int i = 0; i < 4; i++)
pHysiX 21:b642c18eccd1 32 ESC[i].pulsewidth_us(RCCommand[3]);
pHysiX 21:b642c18eccd1 33 } else if (RCCommand[3] > 1150) {
pHysiX 30:d9b988f8d84f 34 if (counterESC) {
pHysiX 21:b642c18eccd1 35 for (int i = 0; i < 3; i++)
pHysiX 21:b642c18eccd1 36 adjust[i] /= 2.0;
pHysiX 30:d9b988f8d84f 37
pHysiX 30:d9b988f8d84f 38 ESCpower[0] = constrainESC((RCCommand[3]) + (adjust[1]) + (adjust[2]) - adjust[0]);
pHysiX 30:d9b988f8d84f 39 ESCpower[1] = constrainESC((RCCommand[3]) + (adjust[1]) - (adjust[2]) + adjust[0]);
pHysiX 30:d9b988f8d84f 40 ESCpower[2] = constrainESC((RCCommand[3]) - (adjust[1]) - (adjust[2]) - adjust[0]);
pHysiX 30:d9b988f8d84f 41 ESCpower[3] = constrainESC((RCCommand[3]) - (adjust[1]) + (adjust[2]) + adjust[0]);
pHysiX 10:ef5fe86f67fe 42
pHysiX 21:b642c18eccd1 43 for (int i = 0; i < 4; i++)
pHysiX 21:b642c18eccd1 44 ESC[i].pulsewidth_us(ESCpower[i]);
pHysiX 10:ef5fe86f67fe 45
pHysiX 21:b642c18eccd1 46 counterESC = false;
pHysiX 21:b642c18eccd1 47 } else {
pHysiX 21:b642c18eccd1 48 stallESC++;
pHysiX 3:605fbcb54e75 49
pHysiX 21:b642c18eccd1 50 if (stallESC > 1) {
pHysiX 21:b642c18eccd1 51 imu.debugSerial.printf("ESC NOT UPDATED FAST ENOUGH!\n");
pHysiX 21:b642c18eccd1 52 stallESC = 0;
pHysiX 21:b642c18eccd1 53 }
pHysiX 21:b642c18eccd1 54 }
pHysiX 10:ef5fe86f67fe 55 } else {
pHysiX 21:b642c18eccd1 56 for (int i = 0; i < 4; i++) {
pHysiX 21:b642c18eccd1 57 ESCpower[i] = 980;
pHysiX 21:b642c18eccd1 58 ESC[i].pulsewidth_us(ESCpower[i]);
pHysiX 10:ef5fe86f67fe 59 }
pHysiX 10:ef5fe86f67fe 60 }
pHysiX 10:ef5fe86f67fe 61 } else {
pHysiX 21:b642c18eccd1 62 if (ESC_check) {
pHysiX 21:b642c18eccd1 63 BT.printf("Need to ARM to check ESC output!\n");
pHysiX 21:b642c18eccd1 64 ESC_check = false;
pHysiX 21:b642c18eccd1 65 }
pHysiX 21:b642c18eccd1 66
pHysiX 21:b642c18eccd1 67 for (int i = 0; i < 4; i++) {
pHysiX 21:b642c18eccd1 68 ESCpower[i] = 980;
pHysiX 10:ef5fe86f67fe 69 ESC[i].pulsewidth_us(ESCpower[i]);
pHysiX 21:b642c18eccd1 70 }
pHysiX 10:ef5fe86f67fe 71 }
pHysiX 3:605fbcb54e75 72
pHysiX 21:b642c18eccd1 73 if (ESC_check)
pHysiX 21:b642c18eccd1 74 BT.printf("%4d %4d %4d %4d\n", ESCpower[0], ESCpower[1], ESCpower[2], ESCpower[3]);
pHysiX 30:d9b988f8d84f 75
pHysiX 30:d9b988f8d84f 76 #ifdef TIME_TASK4
pHysiX 30:d9b988f8d84f 77 _t4.stop();
pHysiX 30:d9b988f8d84f 78 BT.printf("%d\n", _t4.read_us());
pHysiX 30:d9b988f8d84f 79 #endif
pHysiX 19:bd88749c8db4 80 }
pHysiX 19:bd88749c8db4 81
pHysiX 21:b642c18eccd1 82 int constrainESC(float input)
pHysiX 19:bd88749c8db4 83 {
pHysiX 21:b642c18eccd1 84 if (input < 1150.0)
pHysiX 21:b642c18eccd1 85 return 1150;
pHysiX 21:b642c18eccd1 86 else if (input > 2000.0)
pHysiX 19:bd88749c8db4 87 return 2000;
pHysiX 19:bd88749c8db4 88 else
pHysiX 21:b642c18eccd1 89 return (int) input;
pHysiX 19:bd88749c8db4 90 }