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@34:228d87c45151, 2014-05-12 (annotated)

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