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@20:b193a50a2ba3, 2014-05-03 (annotated)

Committer:
pHysiX
Date:
Sat May 03 02:55:24 2014 +0000
Revision:
20:b193a50a2ba3
Parent:
18:af657c4c3944
Child:
21:b642c18eccd1
Added in ability to control PID; fixed PID initialisation bug

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 20:b193a50a2ba3 24
pHysiX 20:b193a50a2ba3 25 if (data == '0' || data == '9') {
pHysiX 20:b193a50a2ba3 26 box_demo = false;
pHysiX 20:b193a50a2ba3 27 rc_out = false;
pHysiX 20:b193a50a2ba3 28 gyro_out = false;
pHysiX 20:b193a50a2ba3 29
pHysiX 20:b193a50a2ba3 30 yawPIDrate.reset();
pHysiX 20:b193a50a2ba3 31 pitchPIDrate.reset();
pHysiX 20:b193a50a2ba3 32 rollPIDrate.reset();
pHysiX 20:b193a50a2ba3 33 } else if (data == 'B') {
pHysiX 4:01921a136f58 34 box_demo = true;
pHysiX 5:4879ef0e6d41 35 rc_out = false;
pHysiX 5:4879ef0e6d41 36 gyro_out = false;
pHysiX 3:605fbcb54e75 37 } else if (data == 'Z') {
pHysiX 5:4879ef0e6d41 38 box_demo = true;
pHysiX 5:4879ef0e6d41 39 rc_out = false;
pHysiX 5:4879ef0e6d41 40 gyro_out = false;
pHysiX 15:10edc6b12122 41 ypr_offset[0] = ypr_use[0];
pHysiX 15:10edc6b12122 42 ypr_offset[1] = ypr_use[1];
pHysiX 15:10edc6b12122 43 ypr_offset[2] = ypr_use[2];
pHysiX 5:4879ef0e6d41 44 } else if (data == 'R') {
pHysiX 5:4879ef0e6d41 45 box_demo = false;
pHysiX 5:4879ef0e6d41 46 rc_out = true;
pHysiX 5:4879ef0e6d41 47 gyro_out = false;
pHysiX 5:4879ef0e6d41 48 } else if (data == 'G') {
pHysiX 3:605fbcb54e75 49 box_demo = false;
pHysiX 5:4879ef0e6d41 50 rc_out = false;
pHysiX 5:4879ef0e6d41 51 gyro_out = true;
pHysiX 20:b193a50a2ba3 52 } else if (data == '1') {
pHysiX 5:4879ef0e6d41 53 box_demo = false;
pHysiX 5:4879ef0e6d41 54 rc_out = false;
pHysiX 5:4879ef0e6d41 55 gyro_out = false;
pHysiX 20:b193a50a2ba3 56 KP_YAW_RATE += 0.1;
pHysiX 20:b193a50a2ba3 57 BT.printf("KP Y Rate: %3.4f\n", KP_YAW_RATE);
pHysiX 20:b193a50a2ba3 58 } else if (data == '2') {
pHysiX 20:b193a50a2ba3 59 box_demo = false;
pHysiX 20:b193a50a2ba3 60 rc_out = false;
pHysiX 20:b193a50a2ba3 61 gyro_out = false;
pHysiX 20:b193a50a2ba3 62 KP_PITCH_RATE += 0.1;
pHysiX 20:b193a50a2ba3 63 BT.printf("KP P Rate: %3.4f\n", KP_PITCH_RATE);
pHysiX 20:b193a50a2ba3 64 } else if (data == '3') {
pHysiX 20:b193a50a2ba3 65 box_demo = false;
pHysiX 20:b193a50a2ba3 66 rc_out = false;
pHysiX 20:b193a50a2ba3 67 gyro_out = false;
pHysiX 20:b193a50a2ba3 68 KP_ROLL_RATE += 0.1;
pHysiX 20:b193a50a2ba3 69 BT.printf("KP R Rate: %3.4f\n", KP_ROLL_RATE);
pHysiX 1:43f8ac7ca6d7 70 }
pHysiX 1:43f8ac7ca6d7 71 }
pHysiX 12:953d25061417 72
pHysiX 5:4879ef0e6d41 73 RCCommand[2] = rxModule[0].pulsewidth(); // Roll
pHysiX 5:4879ef0e6d41 74 RCCommand[3] = rxModule[1].pulsewidth(); // Throttle
pHysiX 5:4879ef0e6d41 75 RCCommand[1] = rxModule[2].pulsewidth(); // Pitch
pHysiX 5:4879ef0e6d41 76 RCCommand[0] = rxModule[3].pulsewidth(); // Yaw
pHysiX 5:4879ef0e6d41 77 RCCommand[4] = rxModule[4].pulsewidth(); // AUX
pHysiX 3:605fbcb54e75 78
pHysiX 5:4879ef0e6d41 79 if (rxModule[1].stallTimer.read_us() > 18820) {
pHysiX 5:4879ef0e6d41 80 for (int i = 0; i < 5; i++)
pHysiX 4:01921a136f58 81 RCCommand[i] = 0;
pHysiX 17:18c3bd016e49 82 } else {
pHysiX 17:18c3bd016e49 83 for (int i = 0; i < 5; i++)
pHysiX 17:18c3bd016e49 84 RCCommand[i] = constrainRCCommand(RCCommand[i]);
pHysiX 3:605fbcb54e75 85 }
pHysiX 5:4879ef0e6d41 86
pHysiX 12:953d25061417 87 if (box_demo) {
pHysiX 12:953d25061417 88 BT.printf("\nV%2.2f\n", voltageSense*VOLTAGE_SCALE);
pHysiX 12:953d25061417 89 BT.printf("\nA%3.2f\nT%2.2f\n", altimeter.Altitude_m(), altimeter.Temp_C());
pHysiX 12:953d25061417 90 } else if (rc_out)
pHysiX 5:4879ef0e6d41 91 BT.printf("%5d %5d %5d %5d %5d\n", RCCommand[0], RCCommand[1], RCCommand[2], RCCommand[3], RCCommand[4]);
pHysiX 12:953d25061417 92
pHysiX 2:ab967d7b4346 93 //LED[3] = !LED[3];
pHysiX 1:43f8ac7ca6d7 94 }
pHysiX 17:18c3bd016e49 95
pHysiX 17:18c3bd016e49 96 int constrainRCCommand(int input)
pHysiX 17:18c3bd016e49 97 {
pHysiX 17:18c3bd016e49 98 if (input < 1000)
pHysiX 17:18c3bd016e49 99 return 1000;
pHysiX 17:18c3bd016e49 100 else if (input > 2000)
pHysiX 17:18c3bd016e49 101 return 2000;
pHysiX 17:18c3bd016e49 102 else
pHysiX 17:18c3bd016e49 103 return input;
pHysiX 17:18c3bd016e49 104 }