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
- Committer:
- pHysiX
- Date:
- 2014-05-12
- Revision:
- 32:7a9be7761c46
- Parent:
- 31:3dde2201e54d
- Child:
- 33:f88a6ee18103
File content as of revision 32:7a9be7761c46:
/* File: Task3.cpp
* Author: Trung Tin Ian HUA
* Date: May 2014
* Purpose: Thread3: RC & BT Command, and Telemetry
* Settings: 50Hz
*/
#define STICK_GAIN 2
#define STICK_GAIN_YAW 16
#include "tasks.h"
#include "setup.h"
#include "PwmIn.h"
PwmIn rxModule[] = {p14, p15, p16, p17, p18};
AnalogIn voltageSense(p20);
bool box_demo = false;
bool rc_out = false;
bool gyro_out = false;
bool command_check = false;
bool adjust_check = false;
/* [YAW PITCH ROLL THROTTLE AUX] */
int RCCommand[5] = {0, 0, 0, 0, 0};
/* Decoded input: [YAW PITCH ROLL] */
int inputYPR[3];
float ypr_offset[3];
float vIn = 0.0;
FLIGHT_MODE mode = ATTITUDE;
//Timer
void Task3(void const *argument)
{
//Timer
if (BT.readable()) {
char data = BT.getc();
uartDecoder(data);
}
/* Receiver decoder: */
RCCommand[2] = rxModule[0].pulsewidth(); // Roll
RCCommand[1] = rxModule[1].pulsewidth(); // Pitch
RCCommand[3] = rxModule[2].pulsewidth(); // Throttle
RCCommand[0] = rxModule[3].pulsewidth(); // Yaw
RCCommand[4] = rxModule[4].pulsewidth(); // AUX
/* Mode switching: */
if (RCCommand[4] > 1500) {
if (mode == RATE) {
} else if (mode == ATTITUDE) {
mode = RATE;
//pitchPIDrate.setTunings(KP_PITCH_RATE, TI_PITCH_RATE, 0.0);
//rollPIDrate.setTunings(KP_ROLL_RATE, TI_ROLL_RATE, 0.0);
} else {}
} else {
if (mode == ATTITUDE) {
} else if (mode == RATE) {
mode = ATTITUDE;
//pitchPIDrate.setTunings(KP_PITCH_RATE, TI_PITCH_RATE, 0.0);
//rollPIDrate.setTunings(KP_ROLL_RATE, TI_PITCH_RATE, 0.0);
}
}
/* Command decoder: */
inputYPR[0] = (RCCommand[0]-1500)*9/100*STICK_GAIN_YAW;
inputYPR[1] = deadbandInputYPR((RCCommand[1]-1500)*-1*9/100*STICK_GAIN);
switch (mode) {
case RATE:
inputYPR[2] = deadbandInputYPR((RCCommand[2]-1500)*9/100*STICK_GAIN);
break;
case ATTITUDE:
default:
inputYPR[2] = deadbandInputYPR((RCCommand[2]-1500)*-1*9/100*STICK_GAIN);
break;
}
/* Lost signal (throttle: */
if (rxModule[2].stallTimer.read_us() > 18820) {
//armed = false;
for (int i = 0; i < 5; i++)
RCCommand[i] = 1000;
} else {
for (int i = 0; i < 5; i++)
RCCommand[i] = constrainRCCommand(RCCommand[i]);
}
if (box_demo) {
BT.printf("\nV%2.2f\n", voltageSense*VOLTAGE_SCALE);
BT.printf("\nA%3.2f\nT%2.2f\n", altimeter.Altitude_m(), altimeter.Temp_C());
} else if (rc_out)
BT.printf("%5d %5d %5d %5d %5d\n", RCCommand[0], RCCommand[1], RCCommand[2], RCCommand[3], RCCommand[4]);
else if (command_check)
BT.printf("%3d %3d %3d\n", inputYPR[0], inputYPR[1], inputYPR[2]);
else {}
//Timer
}
int constrainRCCommand(int input)
{
if (input < 1000)
return 1000;
else if (input > 2000)
return 2000;
else
return input;
}
int deadbandInputYPR(int input)
{
if (input > -2 && input < 4)
return 0;
else
return input;
}
void uartDecoder(char input)
{
switch (input) {
case '9':
case '0':
armed = false;
box_demo = false;
rc_out = false;
gyro_out = false;
ESC_check = false;
command_check = false;
calibration_mode = false;
adjust_check = false;
pitchPIDstable.reset();
rollPIDstable.reset();
yawPIDrate.reset();
pitchPIDrate.reset();
rollPIDrate.reset();
armed? BT.printf("ARMED\n") : BT.printf("DISARMED\n");
break;
case 'D':
case 'd':
armed = false;
box_demo = false;
rc_out = false;
gyro_out = false;
ESC_check = false;
command_check = false;
calibration_mode = false;
adjust_check = false;
ypr_offset[0] = ypr[0];
ypr_offset[1] = ypr[1];
ypr_offset[2] = ypr[2];
pitchPIDstable.reset();
rollPIDstable.reset();
yawPIDrate.reset();
pitchPIDrate.reset();
rollPIDrate.reset();
armed? BT.printf("DISARM FAIL\n") : BT.printf("DISARMED\n");
break;
case 'B':
box_demo = true;
rc_out = false;
gyro_out = false;
ESC_check = false;
command_check = false;
calibration_mode = false;
adjust_check = false;
break;
case 'Z':
ypr_offset[0] = ypr[0];
ypr_offset[1] = ypr[1];
ypr_offset[2] = ypr[2];
break;
case 'R':
box_demo = false;
rc_out = false;
gyro_out = false;
ESC_check = false;
command_check = true;
calibration_mode = false;
adjust_check = false;
break;
case 'r':
box_demo = false;
rc_out = true;
gyro_out = false;
ESC_check = false;
command_check = false;
calibration_mode = false;
adjust_check = false;
break;
case 'G':
case 'g':
box_demo = false;
rc_out = false;
gyro_out = true;
ESC_check = false;
command_check = false;
calibration_mode = false;
adjust_check = false;
break;
case '1':
box_demo = false;
rc_out = false;
gyro_out = false;
ESC_check = false;
KP_YAW_RATE += 0.1;
yawPIDrate.setKP(KP_YAW_RATE);
BT.printf("KP Y rate: %2.5f\n", KP_YAW_RATE);
break;
case 'Q':
case 'q':
box_demo = false;
rc_out = false;
gyro_out = false;
ESC_check = false;
KP_YAW_RATE -= 0.1;
yawPIDrate.setKP(KP_YAW_RATE);
BT.printf("KP Y rate: %2.5f\n", KP_YAW_RATE);
break;
case '2':
box_demo = false;
rc_out = false;
gyro_out = false;
ESC_check = false;
KP_PITCH_RATE += 0.1;
pitchPIDrate.setKP(KP_PITCH_RATE);
BT.printf("KP P rate: %2.5f\n", KP_PITCH_RATE);
break;
case 'W':
case 'w':
box_demo = false;
rc_out = false;
gyro_out = false;
ESC_check = false;
KP_PITCH_RATE -= 0.1;
pitchPIDrate.setKP(KP_PITCH_RATE);
BT.printf("KP P rate: %3.4f\n", KP_PITCH_RATE);
break;
case '3':
box_demo = false;
rc_out = false;
gyro_out = false;
ESC_check = false;
KP_ROLL_RATE += 0.1;
rollPIDrate.setKP(KP_ROLL_RATE);
BT.printf("KP R rate: %3.4f\n", KP_ROLL_RATE);
break;
case 'E':
case 'e':
box_demo = false;
rc_out = false;
gyro_out = false;
ESC_check = false;
KP_ROLL_RATE -= 0.1;
rollPIDrate.setKP(KP_ROLL_RATE);
BT.printf("KP R rate: %2.5f\n", KP_ROLL_RATE);
break;
case '6':
box_demo = false;
rc_out = false;
gyro_out = false;
ESC_check = false;
KP_PITCH_STABLE += 0.1;
pitchPIDstable.setKP(KP_PITCH_STABLE);
BT.printf("KP P stable: %2.5f\n", KP_PITCH_STABLE);
break;
case 'Y':
case 'y':
box_demo = false;
rc_out = false;
gyro_out = false;
ESC_check = false;
KP_PITCH_STABLE -= 0.1;
pitchPIDstable.setKP(KP_PITCH_STABLE);
BT.printf("KP P stable: %2.5f\n", KP_PITCH_STABLE);
break;
case '7':
box_demo = false;
rc_out = false;
gyro_out = false;
ESC_check = false;
KP_ROLL_STABLE += 0.1;
rollPIDstable.setKP(KP_ROLL_STABLE);
BT.printf("KP R stable: %2.5f\n", KP_ROLL_STABLE);
break;
case 'U':
case 'u':
box_demo = false;
rc_out = false;
gyro_out = false;
ESC_check = false;
KP_ROLL_STABLE -= 0.1;
rollPIDstable.setKP(KP_ROLL_STABLE);
BT.printf("KP R stable: %2.5f\n", KP_ROLL_STABLE);
break;
case 'A':
if (!armed) {
if (RCCommand[3] < 1001) {
pitchPIDstable.reset();
rollPIDstable.reset();
yawPIDrate.reset();
pitchPIDrate.reset();
rollPIDrate.reset();
ypr_offset[0] = ypr[0];
ypr_offset[1] = ypr[1];
ypr_offset[2] = ypr[2];
armed = true;
} else {
BT.printf("Check Throttle\n");
}
} else {
BT.printf("ALREADY ARMED!!!\n");
}
box_demo = false;
rc_out = false;
gyro_out = false;
ESC_check = false;
command_check = false;
calibration_mode = false;
adjust_check = false;
armed? BT.printf("ARMED\n"): BT.printf("ARM FAIL\n");
break;
case 'a':
if (armed) {
armed = false;
BT.printf("DISARMED\n");
ypr_offset[0] = ypr[0];
ypr_offset[1] = ypr[1];
ypr_offset[2] = ypr[2];
pitchPIDstable.reset();
rollPIDstable.reset();
} else {
BT.printf("ALREADY DISARMED!!!\n");
}
box_demo = false;
rc_out = false;
gyro_out = false;
ESC_check = false;
command_check = false;
calibration_mode = false;
adjust_check = false;
yawPIDrate.reset();
pitchPIDrate.reset();
rollPIDrate.reset();
armed? BT.printf("DISARM FAIL\n") : BT.printf("DISARMED\n");
break;
case 'P':
box_demo = false;
rc_out = false;
gyro_out = false;
ESC_check = false;
command_check = false;
calibration_mode = false;
adjust_check = true;
break;
case 'p':
box_demo = false;
rc_out = false;
gyro_out = false;
ESC_check = true;
command_check = false;
calibration_mode = false;
adjust_check = false;
break;
case 'C':
case 'c':
box_demo = false;
rc_out = true;
gyro_out = false;
ESC_check = false;
calibration_mode = true;
command_check = false;
adjust_check = false;
BT.printf("Calibration mode...\n");
armed? BT.printf("ARMED\n") : BT.printf("ARM FAILED\n");
break;
case 'M':
case 'm':
switch (mode) {
case RATE:
BT.printf("RATE MODE\n");
break;
case ATTITUDE:
default:
BT.printf("ATTITUDE MODE\n");
break;
}
break;
default:
break;
}
}