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@30:d9b988f8d84f, 2014-05-12 (annotated)

Committer:
pHysiX
Date:
Mon May 12 04:43:38 2014 +0000
Revision:
30:d9b988f8d84f
Parent:
27:18b6580eb0b1
Child:
31:3dde2201e54d
WIP

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