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@22:ef8aa9728013, 2014-05-08 (annotated)

Committer:
pHysiX
Date:
Thu May 08 10:33:43 2014 +0000
Revision:
22:ef8aa9728013
Parent:
21:b642c18eccd1
Child:
24:54a8cdf17378
Commented and tidied entire code for release

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 22:ef8aa9728013 6 */
pHysiX 2:ab967d7b4346 7
pHysiX 21:b642c18eccd1 8 #define STICK_GAIN 2
pHysiX 21:b642c18eccd1 9 #define STICK_GAIN_YAW 4
pHysiX 21:b642c18eccd1 10
pHysiX 1:43f8ac7ca6d7 11 #include "tasks.h"
pHysiX 1:43f8ac7ca6d7 12 #include "setup.h"
pHysiX 4:01921a136f58 13 #include "PwmIn.h"
pHysiX 1:43f8ac7ca6d7 14
pHysiX 1:43f8ac7ca6d7 15 float ypr_offset[3];
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 4:01921a136f58 23 PwmIn rxModule[] = {p14, p15, p16, p17, p18};
pHysiX 12:953d25061417 24 AnalogIn voltageSense(p20);
pHysiX 12:953d25061417 25
pHysiX 12:953d25061417 26 float vIn = 0.0;
pHysiX 3:605fbcb54e75 27
pHysiX 3:605fbcb54e75 28 /* [YAW PITCH ROLL THROTTLE AUX] */
pHysiX 3:605fbcb54e75 29 int RCCommand[5] = {0, 0, 0, 0, 0};
pHysiX 22:ef8aa9728013 30 /* Decoded input: [YAW PITCH ROLL] */
pHysiX 21:b642c18eccd1 31 int inputYPR[3];
pHysiX 1:43f8ac7ca6d7 32
pHysiX 1:43f8ac7ca6d7 33 void Task3(void const *argument)
pHysiX 1:43f8ac7ca6d7 34 {
pHysiX 1:43f8ac7ca6d7 35 if (BT.readable()) {
pHysiX 1:43f8ac7ca6d7 36 char data = BT.getc();
pHysiX 20:b193a50a2ba3 37
pHysiX 21:b642c18eccd1 38 switch (data) {
pHysiX 21:b642c18eccd1 39 case '9':
pHysiX 21:b642c18eccd1 40 case '0':
pHysiX 21:b642c18eccd1 41 armed = false;
pHysiX 21:b642c18eccd1 42 box_demo = false;
pHysiX 21:b642c18eccd1 43 rc_out = false;
pHysiX 21:b642c18eccd1 44 gyro_out = false;
pHysiX 21:b642c18eccd1 45 ESC_check = false;
pHysiX 21:b642c18eccd1 46 command_check = false;
pHysiX 21:b642c18eccd1 47 calibration_mode = false;
pHysiX 21:b642c18eccd1 48 adjust_check = false;
pHysiX 21:b642c18eccd1 49
pHysiX 21:b642c18eccd1 50 pitchPIDstable.reset();
pHysiX 21:b642c18eccd1 51 rollPIDstable.reset();
pHysiX 21:b642c18eccd1 52 yawPIDrate.reset();
pHysiX 21:b642c18eccd1 53 pitchPIDrate.reset();
pHysiX 21:b642c18eccd1 54 rollPIDrate.reset();
pHysiX 21:b642c18eccd1 55
pHysiX 21:b642c18eccd1 56 armed? BT.printf("ARMED\n") : BT.printf("DISARMED\n");
pHysiX 21:b642c18eccd1 57 break;
pHysiX 21:b642c18eccd1 58
pHysiX 21:b642c18eccd1 59 case 'D':
pHysiX 21:b642c18eccd1 60 case 'd':
pHysiX 21:b642c18eccd1 61 armed = false;
pHysiX 21:b642c18eccd1 62 box_demo = false;
pHysiX 21:b642c18eccd1 63 rc_out = false;
pHysiX 21:b642c18eccd1 64 gyro_out = false;
pHysiX 21:b642c18eccd1 65 ESC_check = false;
pHysiX 21:b642c18eccd1 66 command_check = false;
pHysiX 21:b642c18eccd1 67 calibration_mode = false;
pHysiX 21:b642c18eccd1 68 adjust_check = false;
pHysiX 21:b642c18eccd1 69
pHysiX 21:b642c18eccd1 70 ypr_offset[0] = ypr[0];
pHysiX 21:b642c18eccd1 71 ypr_offset[1] = ypr[1];
pHysiX 21:b642c18eccd1 72 ypr_offset[2] = ypr[2];
pHysiX 21:b642c18eccd1 73
pHysiX 21:b642c18eccd1 74 pitchPIDstable.reset();
pHysiX 21:b642c18eccd1 75 rollPIDstable.reset();
pHysiX 21:b642c18eccd1 76 yawPIDrate.reset();
pHysiX 21:b642c18eccd1 77 pitchPIDrate.reset();
pHysiX 21:b642c18eccd1 78 rollPIDrate.reset();
pHysiX 21:b642c18eccd1 79
pHysiX 21:b642c18eccd1 80 armed? BT.printf("DISARM FAIL\n") : BT.printf("DISARMED\n");
pHysiX 21:b642c18eccd1 81 break;
pHysiX 21:b642c18eccd1 82
pHysiX 21:b642c18eccd1 83 case 'B':
pHysiX 21:b642c18eccd1 84 box_demo = true;
pHysiX 21:b642c18eccd1 85 rc_out = false;
pHysiX 21:b642c18eccd1 86 gyro_out = false;
pHysiX 21:b642c18eccd1 87 ESC_check = false;
pHysiX 21:b642c18eccd1 88 command_check = false;
pHysiX 21:b642c18eccd1 89 calibration_mode = false;
pHysiX 21:b642c18eccd1 90 adjust_check = false;
pHysiX 21:b642c18eccd1 91 break;
pHysiX 21:b642c18eccd1 92
pHysiX 21:b642c18eccd1 93 case 'Z':
pHysiX 21:b642c18eccd1 94 ypr_offset[0] = ypr[0];
pHysiX 21:b642c18eccd1 95 ypr_offset[1] = ypr[1];
pHysiX 21:b642c18eccd1 96 ypr_offset[2] = ypr[2];
pHysiX 21:b642c18eccd1 97 break;
pHysiX 21:b642c18eccd1 98
pHysiX 21:b642c18eccd1 99 case 'R':
pHysiX 21:b642c18eccd1 100 box_demo = false;
pHysiX 21:b642c18eccd1 101 rc_out = false;
pHysiX 21:b642c18eccd1 102 gyro_out = false;
pHysiX 21:b642c18eccd1 103 ESC_check = false;
pHysiX 21:b642c18eccd1 104 command_check = true;
pHysiX 21:b642c18eccd1 105 calibration_mode = false;
pHysiX 21:b642c18eccd1 106 adjust_check = false;
pHysiX 21:b642c18eccd1 107 break;
pHysiX 21:b642c18eccd1 108
pHysiX 21:b642c18eccd1 109 case 'r':
pHysiX 21:b642c18eccd1 110 box_demo = false;
pHysiX 21:b642c18eccd1 111 rc_out = true;
pHysiX 21:b642c18eccd1 112 gyro_out = false;
pHysiX 21:b642c18eccd1 113 ESC_check = false;
pHysiX 21:b642c18eccd1 114 command_check = false;
pHysiX 21:b642c18eccd1 115 calibration_mode = false;
pHysiX 21:b642c18eccd1 116 adjust_check = false;
pHysiX 21:b642c18eccd1 117 break;
pHysiX 21:b642c18eccd1 118
pHysiX 21:b642c18eccd1 119 case 'G':
pHysiX 21:b642c18eccd1 120 case 'g':
pHysiX 21:b642c18eccd1 121 box_demo = false;
pHysiX 21:b642c18eccd1 122 rc_out = false;
pHysiX 21:b642c18eccd1 123 gyro_out = true;
pHysiX 21:b642c18eccd1 124 ESC_check = false;
pHysiX 21:b642c18eccd1 125 command_check = false;
pHysiX 21:b642c18eccd1 126 calibration_mode = false;
pHysiX 21:b642c18eccd1 127 adjust_check = false;
pHysiX 21:b642c18eccd1 128 break;
pHysiX 21:b642c18eccd1 129
pHysiX 21:b642c18eccd1 130 case '1':
pHysiX 21:b642c18eccd1 131 box_demo = false;
pHysiX 21:b642c18eccd1 132 rc_out = false;
pHysiX 21:b642c18eccd1 133 gyro_out = false;
pHysiX 21:b642c18eccd1 134 ESC_check = false;
pHysiX 21:b642c18eccd1 135 KP_YAW_RATE += 0.1;
pHysiX 21:b642c18eccd1 136 yawPIDrate.setTunings(KP_YAW_RATE, PID_TI_RATE, 0.0);
pHysiX 21:b642c18eccd1 137 BT.printf("KP Y rate: %2.5f\n", KP_YAW_RATE);
pHysiX 21:b642c18eccd1 138 break;
pHysiX 21:b642c18eccd1 139 case 'Q':
pHysiX 21:b642c18eccd1 140 case 'q':
pHysiX 21:b642c18eccd1 141 box_demo = false;
pHysiX 21:b642c18eccd1 142 rc_out = false;
pHysiX 21:b642c18eccd1 143 gyro_out = false;
pHysiX 21:b642c18eccd1 144 ESC_check = false;
pHysiX 21:b642c18eccd1 145 KP_YAW_RATE -= 0.1;
pHysiX 21:b642c18eccd1 146 yawPIDrate.setTunings(KP_YAW_RATE, PID_TI_RATE, 0.0);
pHysiX 21:b642c18eccd1 147 BT.printf("KP Y rate: %2.5f\n", KP_YAW_RATE);
pHysiX 21:b642c18eccd1 148 break;
pHysiX 20:b193a50a2ba3 149
pHysiX 21:b642c18eccd1 150 case '2':
pHysiX 21:b642c18eccd1 151 box_demo = false;
pHysiX 21:b642c18eccd1 152 rc_out = false;
pHysiX 21:b642c18eccd1 153 gyro_out = false;
pHysiX 21:b642c18eccd1 154 ESC_check = false;
pHysiX 21:b642c18eccd1 155 KP_PITCH_RATE += 0.1;
pHysiX 21:b642c18eccd1 156 pitchPIDrate.setTunings(KP_PITCH_RATE, PID_TI_RATE, 0.0);
pHysiX 21:b642c18eccd1 157 BT.printf("KP P rate: %2.5f\n", KP_PITCH_RATE);
pHysiX 21:b642c18eccd1 158 break;
pHysiX 21:b642c18eccd1 159 case 'W':
pHysiX 21:b642c18eccd1 160 case 'w':
pHysiX 21:b642c18eccd1 161 box_demo = false;
pHysiX 21:b642c18eccd1 162 rc_out = false;
pHysiX 21:b642c18eccd1 163 gyro_out = false;
pHysiX 21:b642c18eccd1 164 ESC_check = false;
pHysiX 21:b642c18eccd1 165 KP_PITCH_RATE -= 0.1;
pHysiX 21:b642c18eccd1 166 pitchPIDrate.setTunings(KP_PITCH_RATE, PID_TI_RATE, 0.0);
pHysiX 21:b642c18eccd1 167 BT.printf("KP P rate: %3.4f\n", KP_PITCH_RATE);
pHysiX 21:b642c18eccd1 168 break;
pHysiX 21:b642c18eccd1 169
pHysiX 21:b642c18eccd1 170 case '3':
pHysiX 21:b642c18eccd1 171 box_demo = false;
pHysiX 21:b642c18eccd1 172 rc_out = false;
pHysiX 21:b642c18eccd1 173 gyro_out = false;
pHysiX 21:b642c18eccd1 174 ESC_check = false;
pHysiX 21:b642c18eccd1 175 KP_ROLL_RATE += 0.1;
pHysiX 21:b642c18eccd1 176 rollPIDrate.setTunings(KP_ROLL_RATE, PID_TI_RATE, 0.0);
pHysiX 21:b642c18eccd1 177 BT.printf("KP R rate: %3.4f\n", KP_ROLL_RATE);
pHysiX 21:b642c18eccd1 178 break;
pHysiX 21:b642c18eccd1 179 case 'E':
pHysiX 21:b642c18eccd1 180 case 'e':
pHysiX 21:b642c18eccd1 181 box_demo = false;
pHysiX 21:b642c18eccd1 182 rc_out = false;
pHysiX 21:b642c18eccd1 183 gyro_out = false;
pHysiX 21:b642c18eccd1 184 ESC_check = false;
pHysiX 21:b642c18eccd1 185 KP_ROLL_RATE -= 0.1;
pHysiX 21:b642c18eccd1 186 rollPIDrate.setTunings(KP_ROLL_RATE, PID_TI_RATE, 0.0);
pHysiX 21:b642c18eccd1 187 BT.printf("KP R rate: %2.5f\n", KP_ROLL_RATE);
pHysiX 21:b642c18eccd1 188 break;
pHysiX 21:b642c18eccd1 189 case 'A':
pHysiX 21:b642c18eccd1 190 if (!armed) {
pHysiX 21:b642c18eccd1 191 pitchPIDstable.reset();
pHysiX 21:b642c18eccd1 192 rollPIDstable.reset();
pHysiX 21:b642c18eccd1 193 yawPIDrate.reset();
pHysiX 21:b642c18eccd1 194 pitchPIDrate.reset();
pHysiX 21:b642c18eccd1 195 rollPIDrate.reset();
pHysiX 21:b642c18eccd1 196
pHysiX 21:b642c18eccd1 197 ypr_offset[0] = ypr[0];
pHysiX 21:b642c18eccd1 198 ypr_offset[1] = ypr[1];
pHysiX 21:b642c18eccd1 199 ypr_offset[2] = ypr[2];
pHysiX 21:b642c18eccd1 200
pHysiX 21:b642c18eccd1 201 armed = true;
pHysiX 21:b642c18eccd1 202 } else {
pHysiX 21:b642c18eccd1 203 BT.printf("ALREADY ARMED!!!\n");
pHysiX 21:b642c18eccd1 204 }
pHysiX 21:b642c18eccd1 205 box_demo = false;
pHysiX 21:b642c18eccd1 206 rc_out = false;
pHysiX 21:b642c18eccd1 207 gyro_out = false;
pHysiX 21:b642c18eccd1 208 ESC_check = false;
pHysiX 21:b642c18eccd1 209 command_check = false;
pHysiX 21:b642c18eccd1 210 calibration_mode = false;
pHysiX 21:b642c18eccd1 211 adjust_check = false;
pHysiX 21:b642c18eccd1 212 armed? BT.printf("ARMED\n"): BT.printf("ARM FAIL\n");
pHysiX 21:b642c18eccd1 213 break;
pHysiX 21:b642c18eccd1 214 case 'a':
pHysiX 21:b642c18eccd1 215 if (armed) {
pHysiX 21:b642c18eccd1 216 armed = false;
pHysiX 21:b642c18eccd1 217 BT.printf("DISARMED\n");
pHysiX 21:b642c18eccd1 218 ypr_offset[0] = ypr[0];
pHysiX 21:b642c18eccd1 219 ypr_offset[1] = ypr[1];
pHysiX 21:b642c18eccd1 220 ypr_offset[2] = ypr[2];
pHysiX 21:b642c18eccd1 221 pitchPIDstable.reset();
pHysiX 21:b642c18eccd1 222 rollPIDstable.reset();
pHysiX 21:b642c18eccd1 223 } else {
pHysiX 21:b642c18eccd1 224 BT.printf("ALREADY DISARMED!!!\n");
pHysiX 21:b642c18eccd1 225 }
pHysiX 21:b642c18eccd1 226 box_demo = false;
pHysiX 21:b642c18eccd1 227 rc_out = false;
pHysiX 21:b642c18eccd1 228 gyro_out = false;
pHysiX 21:b642c18eccd1 229 ESC_check = false;
pHysiX 21:b642c18eccd1 230 command_check = false;
pHysiX 21:b642c18eccd1 231 calibration_mode = false;
pHysiX 21:b642c18eccd1 232 adjust_check = false;
pHysiX 21:b642c18eccd1 233
pHysiX 21:b642c18eccd1 234 yawPIDrate.reset();
pHysiX 21:b642c18eccd1 235 pitchPIDrate.reset();
pHysiX 21:b642c18eccd1 236 rollPIDrate.reset();
pHysiX 21:b642c18eccd1 237
pHysiX 21:b642c18eccd1 238 armed? BT.printf("DISARM FAIL\n") : BT.printf("DISARMED\n");
pHysiX 21:b642c18eccd1 239 break;
pHysiX 21:b642c18eccd1 240
pHysiX 21:b642c18eccd1 241 case 'P':
pHysiX 21:b642c18eccd1 242 box_demo = false;
pHysiX 21:b642c18eccd1 243 rc_out = false;
pHysiX 21:b642c18eccd1 244 gyro_out = false;
pHysiX 21:b642c18eccd1 245 ESC_check = false;
pHysiX 21:b642c18eccd1 246 command_check = false;
pHysiX 21:b642c18eccd1 247 calibration_mode = false;
pHysiX 21:b642c18eccd1 248 adjust_check = true;
pHysiX 21:b642c18eccd1 249 break;
pHysiX 21:b642c18eccd1 250
pHysiX 21:b642c18eccd1 251 case 'p':
pHysiX 21:b642c18eccd1 252 box_demo = false;
pHysiX 21:b642c18eccd1 253 rc_out = false;
pHysiX 21:b642c18eccd1 254 gyro_out = false;
pHysiX 21:b642c18eccd1 255 ESC_check = true;
pHysiX 21:b642c18eccd1 256 command_check = false;
pHysiX 21:b642c18eccd1 257 calibration_mode = false;
pHysiX 21:b642c18eccd1 258 adjust_check = false;
pHysiX 21:b642c18eccd1 259 break;
pHysiX 21:b642c18eccd1 260
pHysiX 21:b642c18eccd1 261 case 'C':
pHysiX 21:b642c18eccd1 262 case 'c':
pHysiX 21:b642c18eccd1 263 box_demo = false;
pHysiX 21:b642c18eccd1 264 rc_out = true;
pHysiX 21:b642c18eccd1 265 gyro_out = false;
pHysiX 21:b642c18eccd1 266 ESC_check = false;
pHysiX 21:b642c18eccd1 267 calibration_mode = true;
pHysiX 21:b642c18eccd1 268 command_check = false;
pHysiX 21:b642c18eccd1 269 adjust_check = false;
pHysiX 21:b642c18eccd1 270
pHysiX 21:b642c18eccd1 271 BT.printf("Calibration mode...\n");
pHysiX 21:b642c18eccd1 272 armed? BT.printf("ARMED\n") : BT.printf("ARM FAILED\n");
pHysiX 21:b642c18eccd1 273 break;
pHysiX 21:b642c18eccd1 274
pHysiX 21:b642c18eccd1 275 default:
pHysiX 21:b642c18eccd1 276 break;
pHysiX 1:43f8ac7ca6d7 277 }
pHysiX 1:43f8ac7ca6d7 278 }
pHysiX 12:953d25061417 279
pHysiX 5:4879ef0e6d41 280 RCCommand[2] = rxModule[0].pulsewidth(); // Roll
pHysiX 21:b642c18eccd1 281 RCCommand[1] = rxModule[1].pulsewidth(); // Pitch
pHysiX 21:b642c18eccd1 282 RCCommand[3] = rxModule[2].pulsewidth(); // Throttle
pHysiX 5:4879ef0e6d41 283 RCCommand[0] = rxModule[3].pulsewidth(); // Yaw
pHysiX 5:4879ef0e6d41 284 RCCommand[4] = rxModule[4].pulsewidth(); // AUX
pHysiX 3:605fbcb54e75 285
pHysiX 21:b642c18eccd1 286 inputYPR[0] = (RCCommand[0]-1500)*9/100*STICK_GAIN_YAW;
pHysiX 21:b642c18eccd1 287 inputYPR[1] = (RCCommand[1]-1500)*-1*9/100*STICK_GAIN;
pHysiX 21:b642c18eccd1 288 inputYPR[2] = (RCCommand[2]-1500)*9/100*STICK_GAIN;
pHysiX 21:b642c18eccd1 289
pHysiX 5:4879ef0e6d41 290 if (rxModule[1].stallTimer.read_us() > 18820) {
pHysiX 21:b642c18eccd1 291 //armed = false;
pHysiX 5:4879ef0e6d41 292 for (int i = 0; i < 5; i++)
pHysiX 4:01921a136f58 293 RCCommand[i] = 0;
pHysiX 17:18c3bd016e49 294 } else {
pHysiX 17:18c3bd016e49 295 for (int i = 0; i < 5; i++)
pHysiX 17:18c3bd016e49 296 RCCommand[i] = constrainRCCommand(RCCommand[i]);
pHysiX 3:605fbcb54e75 297 }
pHysiX 5:4879ef0e6d41 298
pHysiX 12:953d25061417 299 if (box_demo) {
pHysiX 12:953d25061417 300 BT.printf("\nV%2.2f\n", voltageSense*VOLTAGE_SCALE);
pHysiX 12:953d25061417 301 BT.printf("\nA%3.2f\nT%2.2f\n", altimeter.Altitude_m(), altimeter.Temp_C());
pHysiX 12:953d25061417 302 } else if (rc_out)
pHysiX 5:4879ef0e6d41 303 BT.printf("%5d %5d %5d %5d %5d\n", RCCommand[0], RCCommand[1], RCCommand[2], RCCommand[3], RCCommand[4]);
pHysiX 21:b642c18eccd1 304 else if (command_check)
pHysiX 21:b642c18eccd1 305 BT.printf("%3d %3d %3d\n", inputYPR[0], inputYPR[1], inputYPR[2]);
pHysiX 1:43f8ac7ca6d7 306 }
pHysiX 17:18c3bd016e49 307
pHysiX 17:18c3bd016e49 308 int constrainRCCommand(int input)
pHysiX 17:18c3bd016e49 309 {
pHysiX 17:18c3bd016e49 310 if (input < 1000)
pHysiX 17:18c3bd016e49 311 return 1000;
pHysiX 17:18c3bd016e49 312 else if (input > 2000)
pHysiX 17:18c3bd016e49 313 return 2000;
pHysiX 17:18c3bd016e49 314 else
pHysiX 17:18c3bd016e49 315 return input;
pHysiX 17:18c3bd016e49 316 }