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@51:04c6af4319e1, 2014-05-19 (annotated)

Committer:
pHysiX
Date:
Mon May 19 15:30:05 2014 +0000
Revision:
51:04c6af4319e1
Parent:
50:8a0accb23007 
Code rearranged into threads. Timer reserved for time critical routines only. Implemented working semaphore and mutex

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