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.
Diff: RTOS-Threads/src/Task3.cpp
- Revision:
- 21:b642c18eccd1
- Parent:
- 20:b193a50a2ba3
- Child:
- 22:ef8aa9728013
--- a/RTOS-Threads/src/Task3.cpp Sat May 03 02:55:24 2014 +0000
+++ b/RTOS-Threads/src/Task3.cpp Thu May 08 09:39:12 2014 +0000
@@ -1,13 +1,19 @@
/* RC & BT Command & Telemetry (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);
@@ -16,69 +22,270 @@
/* [YAW PITCH ROLL THROTTLE AUX] */
int RCCommand[5] = {0, 0, 0, 0, 0};
+int inputYPR[3];
void Task3(void const *argument)
{
if (BT.readable()) {
char data = BT.getc();
- if (data == '0' || data == '9') {
- box_demo = false;
- rc_out = false;
- gyro_out = false;
+ 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;
- yawPIDrate.reset();
- pitchPIDrate.reset();
- rollPIDrate.reset();
- } else if (data == 'B') {
- box_demo = true;
- rc_out = false;
- gyro_out = false;
- } else if (data == 'Z') {
- box_demo = true;
- rc_out = false;
- gyro_out = false;
- ypr_offset[0] = ypr_use[0];
- ypr_offset[1] = ypr_use[1];
- ypr_offset[2] = ypr_use[2];
- } else if (data == 'R') {
- box_demo = false;
- rc_out = true;
- gyro_out = false;
- } else if (data == 'G') {
- box_demo = false;
- rc_out = false;
- gyro_out = true;
- } else if (data == '1') {
- box_demo = false;
- rc_out = false;
- gyro_out = false;
- KP_YAW_RATE += 0.1;
- BT.printf("KP Y Rate: %3.4f\n", KP_YAW_RATE);
- } else if (data == '2') {
- box_demo = false;
- rc_out = false;
- gyro_out = false;
- KP_PITCH_RATE += 0.1;
- BT.printf("KP P Rate: %3.4f\n", KP_PITCH_RATE);
- } else if (data == '3') {
- box_demo = false;
- rc_out = false;
- gyro_out = false;
- KP_ROLL_RATE += 0.1;
- BT.printf("KP R Rate: %3.4f\n", KP_ROLL_RATE);
+ 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[3] = rxModule[1].pulsewidth(); // Throttle
- RCCommand[1] = rxModule[2].pulsewidth(); // Pitch
+ 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;
+ // Perhaps need to directly control ESCpower[i] instead of writing to RCCommand
} else {
for (int i = 0; i < 5; i++)
RCCommand[i] = constrainRCCommand(RCCommand[i]);
@@ -89,6 +296,8 @@
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]);
//LED[3] = !LED[3];
}