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@48:9dbdc4144f00, 2014-05-19 (annotated)

Committer:
pHysiX
Date:
Mon May 19 13:21:17 2014 +0000
Revision:
48:9dbdc4144f00
Parent:
45:3847d7bf8b2c
Child:
50:8a0accb23007
Altimeter and IMU changed to non-blocking I2C

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