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@40:27e344adfdb9, 2014-05-14 (annotated)

Committer:
pHysiX
Date:
Wed May 14 13:38:19 2014 +0000
Revision:
40:27e344adfdb9
Parent:
39:02782ad251db
Child:
44:4be5c01c6de2
PID stable again. Quadcopter lifting almost straight (off by 0.5deg max on back and left for rate; off by 2deg max on back and left for attitude); Need LPF or better deadbanding since transmitter has noisy sticks; Need Euler instead of YPR angles

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