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:
- 32:7a9be7761c46
- Parent:
- 31:3dde2201e54d
- Child:
- 33:f88a6ee18103
diff -r 3dde2201e54d -r 7a9be7761c46 RTOS-Threads/src/Task3.cpp
--- a/RTOS-Threads/src/Task3.cpp Mon May 12 05:12:19 2014 +0000
+++ b/RTOS-Threads/src/Task3.cpp Mon May 12 13:20:06 2014 +0000
@@ -36,201 +36,282 @@
//Timer
if (BT.readable()) {
char data = BT.getc();
+ uartDecoder(data);
+ }
- 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;
+ /* 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
- pitchPIDstable.reset();
- rollPIDstable.reset();
- yawPIDrate.reset();
- pitchPIDrate.reset();
- rollPIDrate.reset();
-
- armed? BT.printf("ARMED\n") : BT.printf("DISARMED\n");
- break;
+ /* 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);
+ }
+ }
- 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;
+ /* 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;
+ }
- ypr_offset[0] = ypr[0];
- ypr_offset[1] = ypr[1];
- ypr_offset[2] = ypr[2];
+ /* 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]);
+ }
- pitchPIDstable.reset();
- rollPIDstable.reset();
- yawPIDrate.reset();
- pitchPIDrate.reset();
- rollPIDrate.reset();
-
- armed? BT.printf("DISARM FAIL\n") : BT.printf("DISARMED\n");
- break;
+ 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
+}
- case 'B':
- box_demo = true;
- rc_out = false;
- gyro_out = false;
- ESC_check = false;
- command_check = false;
- calibration_mode = false;
- adjust_check = false;
- break;
+int constrainRCCommand(int input)
+{
+ if (input < 1000)
+ return 1000;
+ else if (input > 2000)
+ return 2000;
+ else
+ return input;
+}
- case 'Z':
- ypr_offset[0] = ypr[0];
- ypr_offset[1] = ypr[1];
- ypr_offset[2] = ypr[2];
- break;
+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;
- case 'R':
- box_demo = false;
- rc_out = false;
- gyro_out = false;
- ESC_check = false;
- command_check = true;
- calibration_mode = false;
- adjust_check = false;
- break;
+ 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;
- case 'r':
- box_demo = false;
- rc_out = true;
- gyro_out = false;
- ESC_check = false;
- command_check = false;
- calibration_mode = false;
- adjust_check = false;
- break;
+ 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();
- 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;
+ 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 '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 '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 '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 '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 '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 '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.setTunings(KP_PITCH_STABLE, PID_TI_STABLE, 0.0);
- 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.setTunings(KP_PITCH_STABLE, PID_TI_STABLE, 0.0);
- BT.printf("KP P stable: %2.5f\n", KP_PITCH_STABLE);
- 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.setTunings(KP_ROLL_STABLE, PID_TI_STABLE, 0.0);
- 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.setTunings(KP_ROLL_STABLE, PID_TI_STABLE, 0.0);
- BT.printf("KP R stable: %2.5f\n", KP_ROLL_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) {
+ case 'A':
+ if (!armed) {
+ if (RCCommand[3] < 1001) {
pitchPIDstable.reset();
rollPIDstable.reset();
yawPIDrate.reset();
@@ -243,152 +324,95 @@
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");
+ BT.printf("Check Throttle\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;
+ } 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;
- case 'P':
- box_demo = false;
- rc_out = false;
- gyro_out = false;
- ESC_check = false;
- command_check = false;
- calibration_mode = false;
- adjust_check = true;
- break;
+ yawPIDrate.reset();
+ pitchPIDrate.reset();
+ rollPIDrate.reset();
- case 'p':
- box_demo = false;
- rc_out = false;
- gyro_out = false;
- ESC_check = true;
- command_check = false;
- calibration_mode = false;
- adjust_check = false;
- break;
+ armed? BT.printf("DISARM FAIL\n") : BT.printf("DISARMED\n");
+ 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;
+ case 'P':
+ box_demo = false;
+ rc_out = false;
+ gyro_out = false;
+ ESC_check = false;
+ command_check = false;
+ calibration_mode = false;
+ adjust_check = true;
+ break;
- 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;
+ case 'p':
+ box_demo = false;
+ rc_out = false;
+ gyro_out = false;
+ ESC_check = true;
+ command_check = false;
+ calibration_mode = false;
+ adjust_check = false;
+ break;
- default:
- 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;
- /* 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
-
- /* Throttle decoder: */
- RCCommand[3] = RCCommand[3] * 9/10;
- //RCCommand[3] = (RCCommand[3] + (RCCommand[3]<<3))/10;
+ BT.printf("Calibration mode...\n");
+ armed? BT.printf("ARMED\n") : BT.printf("ARM FAILED\n");
+ break;
- /* Mode switching: */
- if (RCCommand[4] > 1500)
- mode = RATE;
- else
- mode = ATTITUDE;
+ case 'M':
+ case 'm':
+ switch (mode) {
+ case RATE:
+ BT.printf("RATE MODE\n");
+ break;
+ case ATTITUDE:
+ default:
+ BT.printf("ATTITUDE MODE\n");
+ break;
+ }
+ break;
- /* Command decoder: */
- inputYPR[0] = (RCCommand[0]-1500)*9/100*STICK_GAIN_YAW;
- inputYPR[1] = (RCCommand[1]-1500)*-1*9/100*STICK_GAIN;
- switch (mode) {
- case RATE:
- inputYPR[2] = (RCCommand[2]-1500)*9/100*STICK_GAIN;
- break;
- case ATTITUDE:
default:
- inputYPR[2] = (RCCommand[2]-1500)*-1*9/100*STICK_GAIN;
break;
}
-
- 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]);
-
-//Timer
}
-
-int constrainRCCommand(int input)
-{
- if (input < 1000)
- return 1000;
- else if (input > 2000)
- return 2000;
- else
- return input;
-}