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/Task3.cpp@3:605fbcb54e75, 2014-04-29 (annotated)

Committer:
pHysiX
Date:
Tue Apr 29 14:53:32 2014 +0000
Revision:
3:605fbcb54e75
Parent:
2:ab967d7b4346
Child:
4:01921a136f58
Fully implemented system. Need to test stability of RTOS, and to make sure that values are correct. ; Rate Mode only.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pHysiX 2:ab967d7b4346 1 /* RC Command (50Hz) */
pHysiX 2:ab967d7b4346 2
pHysiX 1:43f8ac7ca6d7 3 #include "tasks.h"
pHysiX 1:43f8ac7ca6d7 4 #include "setup.h"
pHysiX 1:43f8ac7ca6d7 5
pHysiX 1:43f8ac7ca6d7 6 float ypr_offset[3];
pHysiX 3:605fbcb54e75 7 bool box_demo = false;
pHysiX 3:605fbcb54e75 8
pHysiX 3:605fbcb54e75 9 Timer stallTime;
pHysiX 3:605fbcb54e75 10 Timer inputTime;
pHysiX 3:605fbcb54e75 11
pHysiX 3:605fbcb54e75 12 AnalogIn rxModule[] = {p16, p17, p18, p19, p20};
pHysiX 3:605fbcb54e75 13
pHysiX 3:605fbcb54e75 14 int rxIn[5] = {0, 0, 0, 0, 0};
pHysiX 3:605fbcb54e75 15
pHysiX 3:605fbcb54e75 16 /* [YAW PITCH ROLL THROTTLE AUX] */
pHysiX 3:605fbcb54e75 17 int RCCommand[5] = {0, 0, 0, 0, 0};
pHysiX 1:43f8ac7ca6d7 18
pHysiX 1:43f8ac7ca6d7 19 void Task3(void const *argument)
pHysiX 1:43f8ac7ca6d7 20 {
pHysiX 1:43f8ac7ca6d7 21 if (BT.readable()) {
pHysiX 1:43f8ac7ca6d7 22 char data = BT.getc();
pHysiX 3:605fbcb54e75 23 if (data == 'B') {
pHysiX 3:605fbcb54e75 24 box_demo = !box_demo;
pHysiX 3:605fbcb54e75 25 } else if (data == 'Z') {
pHysiX 1:43f8ac7ca6d7 26 ypr_offset[0] = ypr[0];
pHysiX 1:43f8ac7ca6d7 27 ypr_offset[1] = ypr[1];
pHysiX 1:43f8ac7ca6d7 28 ypr_offset[2] = ypr[2];
pHysiX 3:605fbcb54e75 29 } else if (data == 'O') {
pHysiX 3:605fbcb54e75 30 box_demo = false;
pHysiX 1:43f8ac7ca6d7 31 }
pHysiX 1:43f8ac7ca6d7 32 }
pHysiX 1:43f8ac7ca6d7 33
pHysiX 3:605fbcb54e75 34 for (int i = 0; i < 5; i++) {
pHysiX 3:605fbcb54e75 35 stallTime.reset();
pHysiX 3:605fbcb54e75 36 inputTime.reset();
pHysiX 3:605fbcb54e75 37
pHysiX 3:605fbcb54e75 38 stallTime.start();
pHysiX 3:605fbcb54e75 39 while (!rxModule[i] && (stallTime.read_us() < 1010)); // wait for high
pHysiX 3:605fbcb54e75 40
pHysiX 3:605fbcb54e75 41 if (stallTime.read_us() < 1050) {
pHysiX 3:605fbcb54e75 42 stallTime.stop();
pHysiX 3:605fbcb54e75 43 stallTime.reset();
pHysiX 3:605fbcb54e75 44
pHysiX 3:605fbcb54e75 45 inputTime.start();
pHysiX 3:605fbcb54e75 46 stallTime.start();
pHysiX 3:605fbcb54e75 47
pHysiX 3:605fbcb54e75 48 while (rxModule[i] && (stallTime.read_us() < 1010)); // wait for low
pHysiX 3:605fbcb54e75 49 if (stallTime.read_us() < 1050) {
pHysiX 3:605fbcb54e75 50 inputTime.stop();
pHysiX 3:605fbcb54e75 51 stallTime.stop();
pHysiX 3:605fbcb54e75 52
pHysiX 3:605fbcb54e75 53 rxIn[i] = inputTime.read_us();
pHysiX 3:605fbcb54e75 54 } else {
pHysiX 3:605fbcb54e75 55 stallTime.stop();
pHysiX 3:605fbcb54e75 56 rxIn[i] = 0;
pHysiX 3:605fbcb54e75 57 }
pHysiX 3:605fbcb54e75 58 } else {
pHysiX 3:605fbcb54e75 59 stallTime.stop();
pHysiX 3:605fbcb54e75 60 rxIn[i] = 0;
pHysiX 3:605fbcb54e75 61 }
pHysiX 3:605fbcb54e75 62 }
pHysiX 3:605fbcb54e75 63
pHysiX 3:605fbcb54e75 64 RCCommand[0] = rxIn[0];
pHysiX 3:605fbcb54e75 65 RCCommand[1] = rxIn[2];
pHysiX 3:605fbcb54e75 66 RCCommand[2] = rxIn[3];
pHysiX 3:605fbcb54e75 67 RCCommand[3] = 1000;//rxIn[1];
pHysiX 3:605fbcb54e75 68 RCCommand[4] = rxIn[4];
pHysiX 3:605fbcb54e75 69
pHysiX 3:605fbcb54e75 70 /*
pHysiX 3:605fbcb54e75 71 for (int i = 0; i < 5; i++)
pHysiX 3:605fbcb54e75 72 RCCommand[i] = 1000;
pHysiX 3:605fbcb54e75 73
pHysiX 3:605fbcb54e75 74 RCCommand[0] = 0;
pHysiX 3:605fbcb54e75 75 RCCommand[1] = 0;
pHysiX 3:605fbcb54e75 76 RCCommand[2] = 0;
pHysiX 3:605fbcb54e75 77 */
pHysiX 3:605fbcb54e75 78
pHysiX 2:ab967d7b4346 79 //LED[3] = !LED[3];
pHysiX 1:43f8ac7ca6d7 80 }