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@54:a36d39a90c21, 2014-05-19 (annotated)

Committer:
pHysiX
Date:
Mon May 19 16:05:11 2014 +0000
Revision:
54:a36d39a90c21
Parent:
51:04c6af4319e1 
PwmInRC library, removed mutex

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 36:d95e3d6f2fc4 5 * Settings: 400Hz
pHysiX 22:ef8aa9728013 6 * 200Hz <= PWM frequency <= 400Hz
pHysiX 36:d95e3d6f2fc4 7 * Frequency: 400Hz
pHysiX 30:d9b988f8d84f 8 * Refer to tasks.h to change PWM frequency
pHysiX 50:8a0accb23007 9 * Timing:
pHysiX 30:d9b988f8d84f 10 */
pHysiX 1:43f8ac7ca6d7 11 #include "tasks.h"
pHysiX 1:43f8ac7ca6d7 12 #include "setup.h"
pHysiX 1:43f8ac7ca6d7 13
pHysiX 50:8a0accb23007 14 Semaphore sem_Task4(1);
pHysiX 50:8a0accb23007 15
pHysiX 44:4be5c01c6de2 16 volatile int ESCpower[4] = {0, 0, 0, 0};
pHysiX 10:ef5fe86f67fe 17 int stallESC = 0;
pHysiX 10:ef5fe86f67fe 18
pHysiX 14:267368c83b6a 19 bool armed = false;
pHysiX 21:b642c18eccd1 20 bool ESC_check = false;
pHysiX 21:b642c18eccd1 21 bool calibration_mode = false;
pHysiX 2:ab967d7b4346 22
pHysiX 34:228d87c45151 23
pHysiX 34:228d87c45151 24
pHysiX 34:228d87c45151 25
pHysiX 34:228d87c45151 26 // ==================
pHysiX 34:228d87c45151 27 // === ESC UPDATE ===
pHysiX 34:228d87c45151 28 // ==================
pHysiX 1:43f8ac7ca6d7 29 void Task4(void const *argurment)
pHysiX 1:43f8ac7ca6d7 30 {
pHysiX 51:04c6af4319e1 31 while (1) {
pHysiX 21:b642c18eccd1 32 if (calibration_mode) {
pHysiX 51:04c6af4319e1 33 if (armed)
pHysiX 51:04c6af4319e1 34 for (int i = 0; i < 4; i++)
pHysiX 51:04c6af4319e1 35 ESC[i].pulsewidth_us(RCCommand[3]);
pHysiX 51:04c6af4319e1 36 } else if (!armed) {
pHysiX 51:04c6af4319e1 37 if (ESC_check) {
pHysiX 51:04c6af4319e1 38 BT.printf("Need to ARM to check ESC output!\n");
pHysiX 51:04c6af4319e1 39 ESC_check = false;
pHysiX 51:04c6af4319e1 40 }
pHysiX 51:04c6af4319e1 41
pHysiX 51:04c6af4319e1 42 for (int i = 0; i < 4; i++) {
pHysiX 51:04c6af4319e1 43 ESCpower[i] = 990;
pHysiX 51:04c6af4319e1 44 ESC[i].pulsewidth_us(ESCpower[i]);
pHysiX 51:04c6af4319e1 45 }
pHysiX 51:04c6af4319e1 46 } else if (armed) {
pHysiX 51:04c6af4319e1 47 if (RCCommand[3] < 1100) {
pHysiX 51:04c6af4319e1 48 for (int i = 0; i < 4; i++) {
pHysiX 51:04c6af4319e1 49 ESCpower[i] = 1000;
pHysiX 51:04c6af4319e1 50 ESC[i].pulsewidth_us(ESCpower[i]);
pHysiX 51:04c6af4319e1 51 }
pHysiX 51:04c6af4319e1 52 } else {
pHysiX 51:04c6af4319e1 53 //PC.printf("T4\n");
pHysiX 51:04c6af4319e1 54 sem_Task4.wait();
pHysiX 51:04c6af4319e1 55 //PC.printf("T4 Sem\n");
pHysiX 51:04c6af4319e1 56
pHysiX 21:b642c18eccd1 57 for (int i = 0; i < 3; i++)
pHysiX 21:b642c18eccd1 58 adjust[i] /= 2.0;
pHysiX 30:d9b988f8d84f 59
pHysiX 32:7a9be7761c46 60 int throttle = RCCommand[3] * 9/10;
pHysiX 32:7a9be7761c46 61
pHysiX 32:7a9be7761c46 62 ESCpower[0] = constrainESC(throttle + (adjust[1]) + (adjust[2]) - adjust[0]);
pHysiX 32:7a9be7761c46 63 ESCpower[1] = constrainESC(throttle + (adjust[1]) - (adjust[2]) + adjust[0]);
pHysiX 32:7a9be7761c46 64 ESCpower[2] = constrainESC(throttle - (adjust[1]) - (adjust[2]) - adjust[0]);
pHysiX 32:7a9be7761c46 65 ESCpower[3] = constrainESC(throttle - (adjust[1]) + (adjust[2]) + adjust[0]);
pHysiX 10:ef5fe86f67fe 66
pHysiX 21:b642c18eccd1 67 for (int i = 0; i < 4; i++)
pHysiX 21:b642c18eccd1 68 ESC[i].pulsewidth_us(ESCpower[i]);
pHysiX 51:04c6af4319e1 69 } // else
pHysiX 51:04c6af4319e1 70 } //else if (armed)
pHysiX 51:04c6af4319e1 71 Thread::wait(TASK4_PERIOD);
pHysiX 51:04c6af4319e1 72 } //while(1)
pHysiX 51:04c6af4319e1 73 } //Task4
pHysiX 19:bd88749c8db4 74
pHysiX 34:228d87c45151 75
pHysiX 34:228d87c45151 76
pHysiX 34:228d87c45151 77
pHysiX 34:228d87c45151 78 // ************************
pHysiX 34:228d87c45151 79 // *** Helper functions ***
pHysiX 34:228d87c45151 80 // ************************
pHysiX 21:b642c18eccd1 81 int constrainESC(float input)
pHysiX 19:bd88749c8db4 82 {
pHysiX 32:7a9be7761c46 83 if (input < 1100.0)
pHysiX 32:7a9be7761c46 84 return 1100;
pHysiX 21:b642c18eccd1 85 else if (input > 2000.0)
pHysiX 19:bd88749c8db4 86 return 2000;
pHysiX 19:bd88749c8db4 87 else
pHysiX 21:b642c18eccd1 88 return (int) input;
pHysiX 19:bd88749c8db4 89 }