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-08
- Revision:
- 22:ef8aa9728013
- Parent:
- 21:b642c18eccd1
- Child:
- 24:54a8cdf17378
File content as of revision 22:ef8aa9728013:
/* 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 4
#include "tasks.h"
#include "setup.h"
#include "PwmIn.h"
float ypr_offset[3];
bool box_demo = false;
bool rc_out = false;
bool gyro_out = false;
bool command_check = false;
bool adjust_check = false;
PwmIn rxModule[] = {p14, p15, p16, p17, p18};
AnalogIn voltageSense(p20);
float vIn = 0.0;
/* [YAW PITCH ROLL THROTTLE AUX] */
int RCCommand[5] = {0, 0, 0, 0, 0};
/* Decoded input: [YAW PITCH ROLL] */
int inputYPR[3];
void Task3(void const *argument)
{
if (BT.readable()) {
char data = BT.getc();
switch (data) {
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.setTunings(KP_YAW_RATE, PID_TI_RATE, 0.0);
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.setTunings(KP_YAW_RATE, PID_TI_RATE, 0.0);
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.setTunings(KP_PITCH_RATE, PID_TI_RATE, 0.0);
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.setTunings(KP_PITCH_RATE, PID_TI_RATE, 0.0);
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.setTunings(KP_ROLL_RATE, PID_TI_RATE, 0.0);
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.setTunings(KP_ROLL_RATE, PID_TI_RATE, 0.0);
BT.printf("KP R rate: %2.5f\n", KP_ROLL_RATE);
break;
case 'A':
if (!armed) {
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("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;
default:
break;
}
}
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
inputYPR[0] = (RCCommand[0]-1500)*9/100*STICK_GAIN_YAW;
inputYPR[1] = (RCCommand[1]-1500)*-1*9/100*STICK_GAIN;
inputYPR[2] = (RCCommand[2]-1500)*9/100*STICK_GAIN;
if (rxModule[1].stallTimer.read_us() > 18820) {
//armed = false;
for (int i = 0; i < 5; i++)
RCCommand[i] = 0;
} 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]);
}
int constrainRCCommand(int input)
{
if (input < 1000)
return 1000;
else if (input > 2000)
return 2000;
else
return input;
}