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@17:18c3bd016e49, 2014-05-03 (annotated)

Committer:
pHysiX
Date:
Sat May 03 01:47:54 2014 +0000
Revision:
17:18c3bd016e49
Parent:
15:10edc6b12122
Child:
18:af657c4c3944
Got rid of extra debug printfs. ; Implemented RCCommand restrain: 1000-2000us

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pHysiX 12:953d25061417 1 /* RC & BT Command & Telemetry (50Hz) */
pHysiX 2:ab967d7b4346 2
pHysiX 1:43f8ac7ca6d7 3 #include "tasks.h"
pHysiX 1:43f8ac7ca6d7 4 #include "setup.h"
pHysiX 4:01921a136f58 5 #include "PwmIn.h"
pHysiX 1:43f8ac7ca6d7 6
pHysiX 1:43f8ac7ca6d7 7 float ypr_offset[3];
pHysiX 3:605fbcb54e75 8 bool box_demo = false;
pHysiX 5:4879ef0e6d41 9 bool rc_out = false;
pHysiX 5:4879ef0e6d41 10 bool gyro_out = false;
pHysiX 3:605fbcb54e75 11
pHysiX 4:01921a136f58 12 PwmIn rxModule[] = {p14, p15, p16, p17, p18};
pHysiX 12:953d25061417 13 AnalogIn voltageSense(p20);
pHysiX 12:953d25061417 14
pHysiX 12:953d25061417 15 float vIn = 0.0;
pHysiX 3:605fbcb54e75 16
pHysiX 3:605fbcb54e75 17 /* [YAW PITCH ROLL THROTTLE AUX] */
pHysiX 3:605fbcb54e75 18 int RCCommand[5] = {0, 0, 0, 0, 0};
pHysiX 1:43f8ac7ca6d7 19
pHysiX 1:43f8ac7ca6d7 20 void Task3(void const *argument)
pHysiX 1:43f8ac7ca6d7 21 {
pHysiX 1:43f8ac7ca6d7 22 if (BT.readable()) {
pHysiX 1:43f8ac7ca6d7 23 char data = BT.getc();
pHysiX 3:605fbcb54e75 24 if (data == 'B') {
pHysiX 4:01921a136f58 25 box_demo = true;
pHysiX 5:4879ef0e6d41 26 rc_out = false;
pHysiX 5:4879ef0e6d41 27 gyro_out = false;
pHysiX 3:605fbcb54e75 28 } else if (data == 'Z') {
pHysiX 5:4879ef0e6d41 29 box_demo = true;
pHysiX 5:4879ef0e6d41 30 rc_out = false;
pHysiX 5:4879ef0e6d41 31 gyro_out = false;
pHysiX 15:10edc6b12122 32 ypr_offset[0] = ypr_use[0];
pHysiX 15:10edc6b12122 33 ypr_offset[1] = ypr_use[1];
pHysiX 15:10edc6b12122 34 ypr_offset[2] = ypr_use[2];
pHysiX 5:4879ef0e6d41 35 } else if (data == 'R') {
pHysiX 5:4879ef0e6d41 36 box_demo = false;
pHysiX 5:4879ef0e6d41 37 rc_out = true;
pHysiX 5:4879ef0e6d41 38 gyro_out = false;
pHysiX 5:4879ef0e6d41 39 } else if (data == 'G') {
pHysiX 3:605fbcb54e75 40 box_demo = false;
pHysiX 5:4879ef0e6d41 41 rc_out = false;
pHysiX 5:4879ef0e6d41 42 gyro_out = true;
pHysiX 5:4879ef0e6d41 43 } else if (data == '0') {
pHysiX 5:4879ef0e6d41 44 box_demo = false;
pHysiX 5:4879ef0e6d41 45 rc_out = false;
pHysiX 5:4879ef0e6d41 46 gyro_out = false;
pHysiX 1:43f8ac7ca6d7 47 }
pHysiX 1:43f8ac7ca6d7 48 }
pHysiX 12:953d25061417 49
pHysiX 5:4879ef0e6d41 50 RCCommand[2] = rxModule[0].pulsewidth(); // Roll
pHysiX 5:4879ef0e6d41 51 RCCommand[3] = rxModule[1].pulsewidth(); // Throttle
pHysiX 5:4879ef0e6d41 52 RCCommand[1] = rxModule[2].pulsewidth(); // Pitch
pHysiX 5:4879ef0e6d41 53 RCCommand[0] = rxModule[3].pulsewidth(); // Yaw
pHysiX 5:4879ef0e6d41 54 RCCommand[4] = rxModule[4].pulsewidth(); // AUX
pHysiX 3:605fbcb54e75 55
pHysiX 5:4879ef0e6d41 56 if (rxModule[1].stallTimer.read_us() > 18820) {
pHysiX 5:4879ef0e6d41 57 for (int i = 0; i < 5; i++)
pHysiX 4:01921a136f58 58 RCCommand[i] = 0;
pHysiX 17:18c3bd016e49 59 } else {
pHysiX 17:18c3bd016e49 60 for (int i = 0; i < 5; i++)
pHysiX 17:18c3bd016e49 61 RCCommand[i] = constrainRCCommand(RCCommand[i]);
pHysiX 3:605fbcb54e75 62 }
pHysiX 5:4879ef0e6d41 63
pHysiX 12:953d25061417 64 if (box_demo) {
pHysiX 12:953d25061417 65 BT.printf("\nV%2.2f\n", voltageSense*VOLTAGE_SCALE);
pHysiX 12:953d25061417 66 BT.printf("\nA%3.2f\nT%2.2f\n", altimeter.Altitude_m(), altimeter.Temp_C());
pHysiX 12:953d25061417 67 } else if (rc_out)
pHysiX 5:4879ef0e6d41 68 BT.printf("%5d %5d %5d %5d %5d\n", RCCommand[0], RCCommand[1], RCCommand[2], RCCommand[3], RCCommand[4]);
pHysiX 12:953d25061417 69
pHysiX 3:605fbcb54e75 70
pHysiX 2:ab967d7b4346 71 //LED[3] = !LED[3];
pHysiX 1:43f8ac7ca6d7 72 }
pHysiX 17:18c3bd016e49 73
pHysiX 17:18c3bd016e49 74 int constrainRCCommand(int input)
pHysiX 17:18c3bd016e49 75 {
pHysiX 17:18c3bd016e49 76 if (input < 1000)
pHysiX 17:18c3bd016e49 77 return 1000;
pHysiX 17:18c3bd016e49 78 else if (input > 2000)
pHysiX 17:18c3bd016e49 79 return 2000;
pHysiX 17:18c3bd016e49 80 else
pHysiX 17:18c3bd016e49 81 return input;
pHysiX 17:18c3bd016e49 82 }