Ian Hua / Quadcopter-mbedRTOS
Committer:
pHysiX
Date:
Wed Apr 30 07:13:11 2014 +0000
Revision:
5:4879ef0e6d41
Parent:
4:01921a136f58
Child:
6:1a5654c14b5b
Fixed bug with gyro and plane axis, added in corrected PID SP and PV

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 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 3:605fbcb54e75 13
pHysiX 3:605fbcb54e75 14 /* [YAW PITCH ROLL THROTTLE AUX] */
pHysiX 3:605fbcb54e75 15 int RCCommand[5] = {0, 0, 0, 0, 0};
pHysiX 1:43f8ac7ca6d7 16
pHysiX 1:43f8ac7ca6d7 17 void Task3(void const *argument)
pHysiX 1:43f8ac7ca6d7 18 {
pHysiX 1:43f8ac7ca6d7 19 if (BT.readable()) {
pHysiX 1:43f8ac7ca6d7 20 char data = BT.getc();
pHysiX 3:605fbcb54e75 21 if (data == 'B') {
pHysiX 4:01921a136f58 22 box_demo = true;
pHysiX 5:4879ef0e6d41 23 rc_out = false;
pHysiX 5:4879ef0e6d41 24 gyro_out = false;
pHysiX 3:605fbcb54e75 25 } else if (data == 'Z') {
pHysiX 5:4879ef0e6d41 26 box_demo = true;
pHysiX 5:4879ef0e6d41 27 rc_out = false;
pHysiX 5:4879ef0e6d41 28 gyro_out = false;
pHysiX 1:43f8ac7ca6d7 29 ypr_offset[0] = ypr[0];
pHysiX 1:43f8ac7ca6d7 30 ypr_offset[1] = ypr[1];
pHysiX 1:43f8ac7ca6d7 31 ypr_offset[2] = ypr[2];
pHysiX 5:4879ef0e6d41 32 } else if (data == 'R') {
pHysiX 5:4879ef0e6d41 33 box_demo = false;
pHysiX 5:4879ef0e6d41 34 rc_out = true;
pHysiX 5:4879ef0e6d41 35 gyro_out = false;
pHysiX 5:4879ef0e6d41 36 } else if (data == 'G') {
pHysiX 3:605fbcb54e75 37 box_demo = false;
pHysiX 5:4879ef0e6d41 38 rc_out = false;
pHysiX 5:4879ef0e6d41 39 gyro_out = true;
pHysiX 5:4879ef0e6d41 40 } else if (data == '0') {
pHysiX 5:4879ef0e6d41 41 box_demo = false;
pHysiX 5:4879ef0e6d41 42 rc_out = false;
pHysiX 5:4879ef0e6d41 43 gyro_out = false;
pHysiX 1:43f8ac7ca6d7 44 }
pHysiX 1:43f8ac7ca6d7 45 }
pHysiX 5:4879ef0e6d41 46 RCCommand[2] = rxModule[0].pulsewidth(); // Roll
pHysiX 5:4879ef0e6d41 47 RCCommand[3] = rxModule[1].pulsewidth(); // Throttle
pHysiX 5:4879ef0e6d41 48 RCCommand[1] = rxModule[2].pulsewidth(); // Pitch
pHysiX 5:4879ef0e6d41 49 RCCommand[0] = rxModule[3].pulsewidth(); // Yaw
pHysiX 5:4879ef0e6d41 50 RCCommand[4] = rxModule[4].pulsewidth(); // AUX
pHysiX 3:605fbcb54e75 51
pHysiX 5:4879ef0e6d41 52 if (rxModule[1].stallTimer.read_us() > 18820) {
pHysiX 5:4879ef0e6d41 53 for (int i = 0; i < 5; i++)
pHysiX 4:01921a136f58 54 RCCommand[i] = 0;
pHysiX 3:605fbcb54e75 55 }
pHysiX 5:4879ef0e6d41 56
pHysiX 5:4879ef0e6d41 57 if (rc_out)
pHysiX 5:4879ef0e6d41 58 BT.printf("%5d %5d %5d %5d %5d\n", RCCommand[0], RCCommand[1], RCCommand[2], RCCommand[3], RCCommand[4]);
pHysiX 3:605fbcb54e75 59
pHysiX 2:ab967d7b4346 60 //LED[3] = !LED[3];
pHysiX 1:43f8ac7ca6d7 61 }