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@47:89a7077a70d3, 2014-05-18 (annotated)

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