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@36:d95e3d6f2fc4, 2014-05-13 (annotated)

Committer:
pHysiX
Date:
Tue May 13 04:05:34 2014 +0000
Revision:
36:d95e3d6f2fc4
Parent:
34:228d87c45151
Child:
37:29feef05d848
Telemetry output completely changed; Code tidied

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pHysiX 22:ef8aa9728013 1 /* File: Task3.cpp
pHysiX 22:ef8aa9728013 2 * Author: Trung Tin Ian HUA
pHysiX 22:ef8aa9728013 3 * Date: May 2014
pHysiX 36:d95e3d6f2fc4 4 * Purpose: Thread3: RC & BT Command
pHysiX 22:ef8aa9728013 5 * Settings: 50Hz
pHysiX 24:54a8cdf17378 6 */
pHysiX 21:b642c18eccd1 7 #define STICK_GAIN 2
pHysiX 25:a7cfe421cb4a 8 #define STICK_GAIN_YAW 16
pHysiX 21:b642c18eccd1 9
pHysiX 1:43f8ac7ca6d7 10 #include "tasks.h"
pHysiX 1:43f8ac7ca6d7 11 #include "setup.h"
pHysiX 4:01921a136f58 12 #include "PwmIn.h"
pHysiX 1:43f8ac7ca6d7 13
pHysiX 30:d9b988f8d84f 14 PwmIn rxModule[] = {p14, p15, p16, p17, p18};
pHysiX 30:d9b988f8d84f 15 AnalogIn voltageSense(p20);
pHysiX 21:b642c18eccd1 16
pHysiX 3:605fbcb54e75 17 /* [YAW PITCH ROLL THROTTLE AUX] */
pHysiX 3:605fbcb54e75 18 int RCCommand[5] = {0, 0, 0, 0, 0};
pHysiX 22:ef8aa9728013 19 /* Decoded input: [YAW PITCH ROLL] */
pHysiX 21:b642c18eccd1 20 int inputYPR[3];
pHysiX 30:d9b988f8d84f 21 float ypr_offset[3];
pHysiX 30:d9b988f8d84f 22
pHysiX 30:d9b988f8d84f 23 float vIn = 0.0;
pHysiX 1:43f8ac7ca6d7 24
pHysiX 27:18b6580eb0b1 25 FLIGHT_MODE mode = ATTITUDE;
pHysiX 24:54a8cdf17378 26
pHysiX 34:228d87c45151 27
pHysiX 34:228d87c45151 28
pHysiX 34:228d87c45151 29
pHysiX 34:228d87c45151 30 // ======================================
pHysiX 34:228d87c45151 31 // === BT & RC COMMAND, AND TELEMETRY ===
pHysiX 34:228d87c45151 32 // ======================================
pHysiX 31:3dde2201e54d 33 //Timer
pHysiX 1:43f8ac7ca6d7 34 void Task3(void const *argument)
pHysiX 1:43f8ac7ca6d7 35 {
pHysiX 31:3dde2201e54d 36 //Timer
pHysiX 1:43f8ac7ca6d7 37 if (BT.readable()) {
pHysiX 1:43f8ac7ca6d7 38 char data = BT.getc();
pHysiX 32:7a9be7761c46 39 uartDecoder(data);
pHysiX 32:7a9be7761c46 40 }
pHysiX 20:b193a50a2ba3 41
pHysiX 32:7a9be7761c46 42 /* Receiver decoder: */
pHysiX 32:7a9be7761c46 43 RCCommand[2] = rxModule[0].pulsewidth(); // Roll
pHysiX 32:7a9be7761c46 44 RCCommand[1] = rxModule[1].pulsewidth(); // Pitch
pHysiX 32:7a9be7761c46 45 RCCommand[3] = rxModule[2].pulsewidth(); // Throttle
pHysiX 32:7a9be7761c46 46 RCCommand[0] = rxModule[3].pulsewidth(); // Yaw
pHysiX 32:7a9be7761c46 47 RCCommand[4] = rxModule[4].pulsewidth(); // AUX
pHysiX 21:b642c18eccd1 48
pHysiX 32:7a9be7761c46 49 /* Mode switching: */
pHysiX 32:7a9be7761c46 50 if (RCCommand[4] > 1500) {
pHysiX 32:7a9be7761c46 51 if (mode == RATE) {
pHysiX 32:7a9be7761c46 52 } else if (mode == ATTITUDE) {
pHysiX 32:7a9be7761c46 53 mode = RATE;
pHysiX 32:7a9be7761c46 54 //pitchPIDrate.setTunings(KP_PITCH_RATE, TI_PITCH_RATE, 0.0);
pHysiX 32:7a9be7761c46 55 //rollPIDrate.setTunings(KP_ROLL_RATE, TI_ROLL_RATE, 0.0);
pHysiX 32:7a9be7761c46 56 } else {}
pHysiX 32:7a9be7761c46 57 } else {
pHysiX 32:7a9be7761c46 58 if (mode == ATTITUDE) {
pHysiX 32:7a9be7761c46 59 } else if (mode == RATE) {
pHysiX 32:7a9be7761c46 60 mode = ATTITUDE;
pHysiX 32:7a9be7761c46 61 //pitchPIDrate.setTunings(KP_PITCH_RATE, TI_PITCH_RATE, 0.0);
pHysiX 32:7a9be7761c46 62 //rollPIDrate.setTunings(KP_ROLL_RATE, TI_PITCH_RATE, 0.0);
pHysiX 32:7a9be7761c46 63 }
pHysiX 32:7a9be7761c46 64 }
pHysiX 21:b642c18eccd1 65
pHysiX 32:7a9be7761c46 66 /* Command decoder: */
pHysiX 32:7a9be7761c46 67 inputYPR[0] = (RCCommand[0]-1500)*9/100*STICK_GAIN_YAW;
pHysiX 32:7a9be7761c46 68 inputYPR[1] = deadbandInputYPR((RCCommand[1]-1500)*-1*9/100*STICK_GAIN);
pHysiX 32:7a9be7761c46 69 switch (mode) {
pHysiX 32:7a9be7761c46 70 case RATE:
pHysiX 32:7a9be7761c46 71 inputYPR[2] = deadbandInputYPR((RCCommand[2]-1500)*9/100*STICK_GAIN);
pHysiX 32:7a9be7761c46 72 break;
pHysiX 32:7a9be7761c46 73 case ATTITUDE:
pHysiX 32:7a9be7761c46 74 default:
pHysiX 32:7a9be7761c46 75 inputYPR[2] = deadbandInputYPR((RCCommand[2]-1500)*-1*9/100*STICK_GAIN);
pHysiX 32:7a9be7761c46 76 break;
pHysiX 32:7a9be7761c46 77 }
pHysiX 21:b642c18eccd1 78
pHysiX 36:d95e3d6f2fc4 79 /* Lost signal (throttle): */
pHysiX 32:7a9be7761c46 80 if (rxModule[2].stallTimer.read_us() > 18820) {
pHysiX 32:7a9be7761c46 81 for (int i = 0; i < 5; i++)
pHysiX 32:7a9be7761c46 82 RCCommand[i] = 1000;
pHysiX 32:7a9be7761c46 83 } else {
pHysiX 32:7a9be7761c46 84 for (int i = 0; i < 5; i++)
pHysiX 32:7a9be7761c46 85 RCCommand[i] = constrainRCCommand(RCCommand[i]);
pHysiX 32:7a9be7761c46 86 }
pHysiX 32:7a9be7761c46 87 //Timer
pHysiX 32:7a9be7761c46 88 }
pHysiX 21:b642c18eccd1 89
pHysiX 34:228d87c45151 90
pHysiX 34:228d87c45151 91
pHysiX 34:228d87c45151 92
pHysiX 34:228d87c45151 93 // ************************
pHysiX 34:228d87c45151 94 // *** Helper functions ***
pHysiX 34:228d87c45151 95 // ************************
pHysiX 32:7a9be7761c46 96 int constrainRCCommand(int input)
pHysiX 32:7a9be7761c46 97 {
pHysiX 32:7a9be7761c46 98 if (input < 1000)
pHysiX 32:7a9be7761c46 99 return 1000;
pHysiX 32:7a9be7761c46 100 else if (input > 2000)
pHysiX 32:7a9be7761c46 101 return 2000;
pHysiX 32:7a9be7761c46 102 else
pHysiX 32:7a9be7761c46 103 return input;
pHysiX 32:7a9be7761c46 104 }
pHysiX 21:b642c18eccd1 105
pHysiX 32:7a9be7761c46 106 int deadbandInputYPR(int input)
pHysiX 32:7a9be7761c46 107 {
pHysiX 32:7a9be7761c46 108 if (input > -2 && input < 4)
pHysiX 32:7a9be7761c46 109 return 0;
pHysiX 32:7a9be7761c46 110 else
pHysiX 32:7a9be7761c46 111 return input;
pHysiX 32:7a9be7761c46 112 }
pHysiX 32:7a9be7761c46 113
pHysiX 32:7a9be7761c46 114 void uartDecoder(char input)
pHysiX 32:7a9be7761c46 115 {
pHysiX 32:7a9be7761c46 116 switch (input) {
pHysiX 32:7a9be7761c46 117 case '9':
pHysiX 32:7a9be7761c46 118 case '0':
pHysiX 32:7a9be7761c46 119 armed = false;
pHysiX 32:7a9be7761c46 120 box_demo = false;
pHysiX 32:7a9be7761c46 121 rc_out = false;
pHysiX 32:7a9be7761c46 122 gyro_out = false;
pHysiX 32:7a9be7761c46 123 ESC_check = false;
pHysiX 32:7a9be7761c46 124 command_check = false;
pHysiX 32:7a9be7761c46 125 calibration_mode = false;
pHysiX 32:7a9be7761c46 126 adjust_check = false;
pHysiX 21:b642c18eccd1 127
pHysiX 32:7a9be7761c46 128 pitchPIDstable.reset();
pHysiX 32:7a9be7761c46 129 rollPIDstable.reset();
pHysiX 32:7a9be7761c46 130 yawPIDrate.reset();
pHysiX 32:7a9be7761c46 131 pitchPIDrate.reset();
pHysiX 32:7a9be7761c46 132 rollPIDrate.reset();
pHysiX 32:7a9be7761c46 133
pHysiX 32:7a9be7761c46 134 armed? BT.printf("ARMED\n") : BT.printf("DISARMED\n");
pHysiX 32:7a9be7761c46 135 break;
pHysiX 32:7a9be7761c46 136
pHysiX 32:7a9be7761c46 137 case 'D':
pHysiX 32:7a9be7761c46 138 case 'd':
pHysiX 32:7a9be7761c46 139 armed = false;
pHysiX 32:7a9be7761c46 140 box_demo = false;
pHysiX 32:7a9be7761c46 141 rc_out = false;
pHysiX 32:7a9be7761c46 142 gyro_out = false;
pHysiX 32:7a9be7761c46 143 ESC_check = false;
pHysiX 32:7a9be7761c46 144 command_check = false;
pHysiX 32:7a9be7761c46 145 calibration_mode = false;
pHysiX 32:7a9be7761c46 146 adjust_check = false;
pHysiX 21:b642c18eccd1 147
pHysiX 32:7a9be7761c46 148 ypr_offset[0] = ypr[0];
pHysiX 32:7a9be7761c46 149 ypr_offset[1] = ypr[1];
pHysiX 32:7a9be7761c46 150 ypr_offset[2] = ypr[2];
pHysiX 32:7a9be7761c46 151
pHysiX 32:7a9be7761c46 152 pitchPIDstable.reset();
pHysiX 32:7a9be7761c46 153 rollPIDstable.reset();
pHysiX 32:7a9be7761c46 154 yawPIDrate.reset();
pHysiX 32:7a9be7761c46 155 pitchPIDrate.reset();
pHysiX 32:7a9be7761c46 156 rollPIDrate.reset();
pHysiX 21:b642c18eccd1 157
pHysiX 32:7a9be7761c46 158 armed? BT.printf("DISARM FAIL\n") : BT.printf("DISARMED\n");
pHysiX 32:7a9be7761c46 159 break;
pHysiX 32:7a9be7761c46 160
pHysiX 32:7a9be7761c46 161 case 'B':
pHysiX 32:7a9be7761c46 162 box_demo = true;
pHysiX 32:7a9be7761c46 163 rc_out = false;
pHysiX 32:7a9be7761c46 164 gyro_out = false;
pHysiX 32:7a9be7761c46 165 ESC_check = false;
pHysiX 32:7a9be7761c46 166 command_check = false;
pHysiX 32:7a9be7761c46 167 calibration_mode = false;
pHysiX 32:7a9be7761c46 168 adjust_check = false;
pHysiX 32:7a9be7761c46 169 break;
pHysiX 21:b642c18eccd1 170
pHysiX 32:7a9be7761c46 171 case 'Z':
pHysiX 32:7a9be7761c46 172 ypr_offset[0] = ypr[0];
pHysiX 32:7a9be7761c46 173 ypr_offset[1] = ypr[1];
pHysiX 32:7a9be7761c46 174 ypr_offset[2] = ypr[2];
pHysiX 32:7a9be7761c46 175 break;
pHysiX 32:7a9be7761c46 176
pHysiX 32:7a9be7761c46 177 case 'R':
pHysiX 32:7a9be7761c46 178 box_demo = false;
pHysiX 32:7a9be7761c46 179 rc_out = false;
pHysiX 32:7a9be7761c46 180 gyro_out = false;
pHysiX 32:7a9be7761c46 181 ESC_check = false;
pHysiX 32:7a9be7761c46 182 command_check = true;
pHysiX 32:7a9be7761c46 183 calibration_mode = false;
pHysiX 32:7a9be7761c46 184 adjust_check = false;
pHysiX 32:7a9be7761c46 185 break;
pHysiX 32:7a9be7761c46 186
pHysiX 32:7a9be7761c46 187 case 'r':
pHysiX 32:7a9be7761c46 188 box_demo = false;
pHysiX 32:7a9be7761c46 189 rc_out = true;
pHysiX 32:7a9be7761c46 190 gyro_out = false;
pHysiX 32:7a9be7761c46 191 ESC_check = false;
pHysiX 32:7a9be7761c46 192 command_check = false;
pHysiX 32:7a9be7761c46 193 calibration_mode = false;
pHysiX 32:7a9be7761c46 194 adjust_check = false;
pHysiX 32:7a9be7761c46 195 break;
pHysiX 20:b193a50a2ba3 196
pHysiX 32:7a9be7761c46 197 case 'G':
pHysiX 32:7a9be7761c46 198 case 'g':
pHysiX 32:7a9be7761c46 199 box_demo = false;
pHysiX 32:7a9be7761c46 200 rc_out = false;
pHysiX 32:7a9be7761c46 201 gyro_out = true;
pHysiX 32:7a9be7761c46 202 ESC_check = false;
pHysiX 32:7a9be7761c46 203 command_check = false;
pHysiX 32:7a9be7761c46 204 calibration_mode = false;
pHysiX 32:7a9be7761c46 205 adjust_check = false;
pHysiX 32:7a9be7761c46 206 break;
pHysiX 32:7a9be7761c46 207
pHysiX 32:7a9be7761c46 208 case '1':
pHysiX 32:7a9be7761c46 209 box_demo = false;
pHysiX 32:7a9be7761c46 210 rc_out = false;
pHysiX 32:7a9be7761c46 211 gyro_out = false;
pHysiX 32:7a9be7761c46 212 ESC_check = false;
pHysiX 32:7a9be7761c46 213 KP_YAW_RATE += 0.1;
pHysiX 32:7a9be7761c46 214 yawPIDrate.setKP(KP_YAW_RATE);
pHysiX 32:7a9be7761c46 215 BT.printf("KP Y rate: %2.5f\n", KP_YAW_RATE);
pHysiX 32:7a9be7761c46 216 break;
pHysiX 32:7a9be7761c46 217 case 'Q':
pHysiX 32:7a9be7761c46 218 case 'q':
pHysiX 32:7a9be7761c46 219 box_demo = false;
pHysiX 32:7a9be7761c46 220 rc_out = false;
pHysiX 32:7a9be7761c46 221 gyro_out = false;
pHysiX 32:7a9be7761c46 222 ESC_check = false;
pHysiX 32:7a9be7761c46 223 KP_YAW_RATE -= 0.1;
pHysiX 32:7a9be7761c46 224 yawPIDrate.setKP(KP_YAW_RATE);
pHysiX 32:7a9be7761c46 225 BT.printf("KP Y rate: %2.5f\n", KP_YAW_RATE);
pHysiX 32:7a9be7761c46 226 break;
pHysiX 21:b642c18eccd1 227
pHysiX 32:7a9be7761c46 228 case '2':
pHysiX 32:7a9be7761c46 229 box_demo = false;
pHysiX 32:7a9be7761c46 230 rc_out = false;
pHysiX 32:7a9be7761c46 231 gyro_out = false;
pHysiX 32:7a9be7761c46 232 ESC_check = false;
pHysiX 32:7a9be7761c46 233 KP_PITCH_RATE += 0.1;
pHysiX 32:7a9be7761c46 234 pitchPIDrate.setKP(KP_PITCH_RATE);
pHysiX 32:7a9be7761c46 235 BT.printf("KP P rate: %2.5f\n", KP_PITCH_RATE);
pHysiX 32:7a9be7761c46 236 break;
pHysiX 32:7a9be7761c46 237 case 'W':
pHysiX 32:7a9be7761c46 238 case 'w':
pHysiX 32:7a9be7761c46 239 box_demo = false;
pHysiX 32:7a9be7761c46 240 rc_out = false;
pHysiX 32:7a9be7761c46 241 gyro_out = false;
pHysiX 32:7a9be7761c46 242 ESC_check = false;
pHysiX 32:7a9be7761c46 243 KP_PITCH_RATE -= 0.1;
pHysiX 32:7a9be7761c46 244 pitchPIDrate.setKP(KP_PITCH_RATE);
pHysiX 32:7a9be7761c46 245 BT.printf("KP P rate: %3.4f\n", KP_PITCH_RATE);
pHysiX 32:7a9be7761c46 246 break;
pHysiX 32:7a9be7761c46 247
pHysiX 32:7a9be7761c46 248 case '3':
pHysiX 32:7a9be7761c46 249 box_demo = false;
pHysiX 32:7a9be7761c46 250 rc_out = false;
pHysiX 32:7a9be7761c46 251 gyro_out = false;
pHysiX 32:7a9be7761c46 252 ESC_check = false;
pHysiX 32:7a9be7761c46 253 KP_ROLL_RATE += 0.1;
pHysiX 32:7a9be7761c46 254 rollPIDrate.setKP(KP_ROLL_RATE);
pHysiX 32:7a9be7761c46 255 BT.printf("KP R rate: %3.4f\n", KP_ROLL_RATE);
pHysiX 32:7a9be7761c46 256 break;
pHysiX 32:7a9be7761c46 257 case 'E':
pHysiX 32:7a9be7761c46 258 case 'e':
pHysiX 32:7a9be7761c46 259 box_demo = false;
pHysiX 32:7a9be7761c46 260 rc_out = false;
pHysiX 32:7a9be7761c46 261 gyro_out = false;
pHysiX 32:7a9be7761c46 262 ESC_check = false;
pHysiX 32:7a9be7761c46 263 KP_ROLL_RATE -= 0.1;
pHysiX 32:7a9be7761c46 264 rollPIDrate.setKP(KP_ROLL_RATE);
pHysiX 32:7a9be7761c46 265 BT.printf("KP R rate: %2.5f\n", KP_ROLL_RATE);
pHysiX 32:7a9be7761c46 266 break;
pHysiX 24:54a8cdf17378 267
pHysiX 32:7a9be7761c46 268 case '6':
pHysiX 32:7a9be7761c46 269 box_demo = false;
pHysiX 32:7a9be7761c46 270 rc_out = false;
pHysiX 32:7a9be7761c46 271 gyro_out = false;
pHysiX 32:7a9be7761c46 272 ESC_check = false;
pHysiX 32:7a9be7761c46 273 KP_PITCH_STABLE += 0.1;
pHysiX 32:7a9be7761c46 274 pitchPIDstable.setKP(KP_PITCH_STABLE);
pHysiX 32:7a9be7761c46 275 BT.printf("KP P stable: %2.5f\n", KP_PITCH_STABLE);
pHysiX 32:7a9be7761c46 276 break;
pHysiX 32:7a9be7761c46 277 case 'Y':
pHysiX 32:7a9be7761c46 278 case 'y':
pHysiX 32:7a9be7761c46 279 box_demo = false;
pHysiX 32:7a9be7761c46 280 rc_out = false;
pHysiX 32:7a9be7761c46 281 gyro_out = false;
pHysiX 32:7a9be7761c46 282 ESC_check = false;
pHysiX 32:7a9be7761c46 283 KP_PITCH_STABLE -= 0.1;
pHysiX 32:7a9be7761c46 284 pitchPIDstable.setKP(KP_PITCH_STABLE);
pHysiX 32:7a9be7761c46 285 BT.printf("KP P stable: %2.5f\n", KP_PITCH_STABLE);
pHysiX 32:7a9be7761c46 286 break;
pHysiX 24:54a8cdf17378 287
pHysiX 32:7a9be7761c46 288 case '7':
pHysiX 32:7a9be7761c46 289 box_demo = false;
pHysiX 32:7a9be7761c46 290 rc_out = false;
pHysiX 32:7a9be7761c46 291 gyro_out = false;
pHysiX 32:7a9be7761c46 292 ESC_check = false;
pHysiX 32:7a9be7761c46 293 KP_ROLL_STABLE += 0.1;
pHysiX 32:7a9be7761c46 294 rollPIDstable.setKP(KP_ROLL_STABLE);
pHysiX 32:7a9be7761c46 295 BT.printf("KP R stable: %2.5f\n", KP_ROLL_STABLE);
pHysiX 32:7a9be7761c46 296 break;
pHysiX 32:7a9be7761c46 297 case 'U':
pHysiX 32:7a9be7761c46 298 case 'u':
pHysiX 32:7a9be7761c46 299 box_demo = false;
pHysiX 32:7a9be7761c46 300 rc_out = false;
pHysiX 32:7a9be7761c46 301 gyro_out = false;
pHysiX 32:7a9be7761c46 302 ESC_check = false;
pHysiX 32:7a9be7761c46 303 KP_ROLL_STABLE -= 0.1;
pHysiX 32:7a9be7761c46 304 rollPIDstable.setKP(KP_ROLL_STABLE);
pHysiX 32:7a9be7761c46 305 BT.printf("KP R stable: %2.5f\n", KP_ROLL_STABLE);
pHysiX 32:7a9be7761c46 306 break;
pHysiX 24:54a8cdf17378 307
pHysiX 32:7a9be7761c46 308 case 'A':
pHysiX 32:7a9be7761c46 309 if (!armed) {
pHysiX 32:7a9be7761c46 310 if (RCCommand[3] < 1001) {
pHysiX 21:b642c18eccd1 311 pitchPIDstable.reset();
pHysiX 21:b642c18eccd1 312 rollPIDstable.reset();
pHysiX 21:b642c18eccd1 313 yawPIDrate.reset();
pHysiX 21:b642c18eccd1 314 pitchPIDrate.reset();
pHysiX 21:b642c18eccd1 315 rollPIDrate.reset();
pHysiX 21:b642c18eccd1 316
pHysiX 21:b642c18eccd1 317 ypr_offset[0] = ypr[0];
pHysiX 21:b642c18eccd1 318 ypr_offset[1] = ypr[1];
pHysiX 21:b642c18eccd1 319 ypr_offset[2] = ypr[2];
pHysiX 21:b642c18eccd1 320
pHysiX 21:b642c18eccd1 321 armed = true;
pHysiX 21:b642c18eccd1 322 } else {
pHysiX 32:7a9be7761c46 323 BT.printf("Check Throttle\n");
pHysiX 21:b642c18eccd1 324 }
pHysiX 32:7a9be7761c46 325 } else {
pHysiX 32:7a9be7761c46 326 BT.printf("ALREADY ARMED!!!\n");
pHysiX 32:7a9be7761c46 327 }
pHysiX 32:7a9be7761c46 328 box_demo = false;
pHysiX 32:7a9be7761c46 329 rc_out = false;
pHysiX 32:7a9be7761c46 330 gyro_out = false;
pHysiX 32:7a9be7761c46 331 ESC_check = false;
pHysiX 32:7a9be7761c46 332 command_check = false;
pHysiX 32:7a9be7761c46 333 calibration_mode = false;
pHysiX 32:7a9be7761c46 334 adjust_check = false;
pHysiX 32:7a9be7761c46 335 armed? BT.printf("ARMED\n"): BT.printf("ARM FAIL\n");
pHysiX 32:7a9be7761c46 336 break;
pHysiX 32:7a9be7761c46 337 case 'a':
pHysiX 32:7a9be7761c46 338 if (armed) {
pHysiX 32:7a9be7761c46 339 armed = false;
pHysiX 32:7a9be7761c46 340 BT.printf("DISARMED\n");
pHysiX 32:7a9be7761c46 341 ypr_offset[0] = ypr[0];
pHysiX 32:7a9be7761c46 342 ypr_offset[1] = ypr[1];
pHysiX 32:7a9be7761c46 343 ypr_offset[2] = ypr[2];
pHysiX 32:7a9be7761c46 344 pitchPIDstable.reset();
pHysiX 32:7a9be7761c46 345 rollPIDstable.reset();
pHysiX 32:7a9be7761c46 346 } else {
pHysiX 32:7a9be7761c46 347 BT.printf("ALREADY DISARMED!!!\n");
pHysiX 32:7a9be7761c46 348 }
pHysiX 32:7a9be7761c46 349 box_demo = false;
pHysiX 32:7a9be7761c46 350 rc_out = false;
pHysiX 32:7a9be7761c46 351 gyro_out = false;
pHysiX 32:7a9be7761c46 352 ESC_check = false;
pHysiX 32:7a9be7761c46 353 command_check = false;
pHysiX 32:7a9be7761c46 354 calibration_mode = false;
pHysiX 32:7a9be7761c46 355 adjust_check = false;
pHysiX 21:b642c18eccd1 356
pHysiX 32:7a9be7761c46 357 yawPIDrate.reset();
pHysiX 32:7a9be7761c46 358 pitchPIDrate.reset();
pHysiX 32:7a9be7761c46 359 rollPIDrate.reset();
pHysiX 21:b642c18eccd1 360
pHysiX 32:7a9be7761c46 361 armed? BT.printf("DISARM FAIL\n") : BT.printf("DISARMED\n");
pHysiX 32:7a9be7761c46 362 break;
pHysiX 21:b642c18eccd1 363
pHysiX 32:7a9be7761c46 364 case 'P':
pHysiX 32:7a9be7761c46 365 box_demo = false;
pHysiX 32:7a9be7761c46 366 rc_out = false;
pHysiX 32:7a9be7761c46 367 gyro_out = false;
pHysiX 32:7a9be7761c46 368 ESC_check = false;
pHysiX 32:7a9be7761c46 369 command_check = false;
pHysiX 32:7a9be7761c46 370 calibration_mode = false;
pHysiX 32:7a9be7761c46 371 adjust_check = true;
pHysiX 32:7a9be7761c46 372 break;
pHysiX 21:b642c18eccd1 373
pHysiX 32:7a9be7761c46 374 case 'p':
pHysiX 32:7a9be7761c46 375 box_demo = false;
pHysiX 32:7a9be7761c46 376 rc_out = false;
pHysiX 32:7a9be7761c46 377 gyro_out = false;
pHysiX 32:7a9be7761c46 378 ESC_check = true;
pHysiX 32:7a9be7761c46 379 command_check = false;
pHysiX 32:7a9be7761c46 380 calibration_mode = false;
pHysiX 32:7a9be7761c46 381 adjust_check = false;
pHysiX 32:7a9be7761c46 382 break;
pHysiX 25:a7cfe421cb4a 383
pHysiX 32:7a9be7761c46 384 case 'C':
pHysiX 32:7a9be7761c46 385 case 'c':
pHysiX 32:7a9be7761c46 386 box_demo = false;
pHysiX 32:7a9be7761c46 387 rc_out = true;
pHysiX 32:7a9be7761c46 388 gyro_out = false;
pHysiX 32:7a9be7761c46 389 ESC_check = false;
pHysiX 32:7a9be7761c46 390 calibration_mode = true;
pHysiX 32:7a9be7761c46 391 command_check = false;
pHysiX 32:7a9be7761c46 392 adjust_check = false;
pHysiX 12:953d25061417 393
pHysiX 32:7a9be7761c46 394 BT.printf("Calibration mode...\n");
pHysiX 32:7a9be7761c46 395 armed? BT.printf("ARMED\n") : BT.printf("ARM FAILED\n");
pHysiX 32:7a9be7761c46 396 break;
pHysiX 3:605fbcb54e75 397
pHysiX 32:7a9be7761c46 398 case 'M':
pHysiX 32:7a9be7761c46 399 case 'm':
pHysiX 32:7a9be7761c46 400 switch (mode) {
pHysiX 32:7a9be7761c46 401 case RATE:
pHysiX 32:7a9be7761c46 402 BT.printf("RATE MODE\n");
pHysiX 32:7a9be7761c46 403 break;
pHysiX 32:7a9be7761c46 404 case ATTITUDE:
pHysiX 32:7a9be7761c46 405 default:
pHysiX 32:7a9be7761c46 406 BT.printf("ATTITUDE MODE\n");
pHysiX 32:7a9be7761c46 407 break;
pHysiX 32:7a9be7761c46 408 }
pHysiX 32:7a9be7761c46 409 break;
pHysiX 24:54a8cdf17378 410
pHysiX 25:a7cfe421cb4a 411 default:
pHysiX 25:a7cfe421cb4a 412 break;
pHysiX 25:a7cfe421cb4a 413 }
pHysiX 1:43f8ac7ca6d7 414 }