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

Committer:
pHysiX
Date:
Thu May 08 13:35:13 2014 +0000
Revision:
25:a7cfe421cb4a
Parent:
24:54a8cdf17378
Child:
27:18b6580eb0b1
Added RATE and STABLE mode switch.; Tuned Primary and Secondary PID loops.; Quadcopter can now be flown stably indoors.; Need to add in Integral Control, and then clean up to move away from Bluetooth arming.

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