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/Task2.cpp@18:af657c4c3944, 2014-05-03 (annotated)

Committer:
pHysiX
Date:
Sat May 03 02:05:51 2014 +0000
Revision:
18:af657c4c3944
Parent:
17:18c3bd016e49
Child:
21:b642c18eccd1
Better FIFO handling: Only keep max of 2 packets in FIFO, read all of FIFO each time FIFO is read

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pHysiX 12:953d25061417 1 /* Gyro & PID (200Hz) */
pHysiX 2:ab967d7b4346 2
pHysiX 1:43f8ac7ca6d7 3 #include "Task2.h"
pHysiX 1:43f8ac7ca6d7 4 #include "setup.h"
pHysiX 3:605fbcb54e75 5 #include "PID.h"
pHysiX 3:605fbcb54e75 6
pHysiX 3:605fbcb54e75 7 int yaw_adjust = 0;
pHysiX 3:605fbcb54e75 8 int pitch_adjust = 0;
pHysiX 3:605fbcb54e75 9 int roll_adjust = 0;
pHysiX 3:605fbcb54e75 10
pHysiX 3:605fbcb54e75 11 int16_t gx, gy, gz;
pHysiX 10:ef5fe86f67fe 12 volatile int gyro[3] = {0, 0, 0};
pHysiX 10:ef5fe86f67fe 13
pHysiX 10:ef5fe86f67fe 14 bool counterESC = false;
pHysiX 1:43f8ac7ca6d7 15
pHysiX 1:43f8ac7ca6d7 16 void Task2(void const *argument)
pHysiX 1:43f8ac7ca6d7 17 {
pHysiX 17:18c3bd016e49 18
pHysiX 17:18c3bd016e49 19 /*
pHysiX 17:18c3bd016e49 20 if (counterTask1 > 1) {
pHysiX 17:18c3bd016e49 21 //yawPIDstable.setSetPoint((RCCommand[0]-1500)*9/100);
pHysiX 17:18c3bd016e49 22 pitchPIDstable.setSetPoint(RCCommand[1]-1500)*9/100);
pHysiX 17:18c3bd016e49 23 rollPIDstable.setSetPoint(RCCommand[2]-1500)*9/100);
pHysiX 17:18c3bd016e49 24
pHysiX 17:18c3bd016e49 25 //yawPIDstable.setProcessValue(ypr[0]);
pHysiX 17:18c3bd016e49 26 pitchPIDstable.setProcessValue(ypr[1]);
pHysiX 17:18c3bd016e49 27 rollPIDstable.setProcessValue(ypr[2]);
pHysiX 17:18c3bd016e49 28
pHysiX 17:18c3bd016e49 29 counterTask1 = 0;
pHysiX 17:18c3bd016e49 30
pHysiX 17:18c3bd016e49 31 pitch_adjust_stable = pitchPIDstable.compute();
pHysiX 17:18c3bd016e49 32 roll_adjust_stable = rollPIDstable.compute();
pHysiX 17:18c3bd016e49 33
pHysiX 17:18c3bd016e49 34 yawPIDrate.setSetPoint((RCCommand[0]-1500)*9/100);
pHysiX 17:18c3bd016e49 35 pitchPIDrate.setSetPoint(pitch_adjust_stable);
pHysiX 17:18c3bd016e49 36 rollPIDrate.setSetPoint(rate_adjust_stable);
pHysiX 17:18c3bd016e49 37
pHysiX 17:18c3bd016e49 38 imu.getRotation(&gx, &gy, &gz);
pHysiX 17:18c3bd016e49 39 gyro[0] = ((float) gx / 32.8) + 2;
pHysiX 17:18c3bd016e49 40 gyro[1] = ((float) gy / 32.8);
pHysiX 17:18c3bd016e49 41 gyro[2] = ((float) gz / 32.8);
pHysiX 17:18c3bd016e49 42
pHysiX 17:18c3bd016e49 43 yawPIDrate.setProcessValue(gyro[2]);
pHysiX 17:18c3bd016e49 44 pitchPIDrate.setProcessValue(gyro[1]);
pHysiX 17:18c3bd016e49 45 rollPIDrate.setProcessValue(gyro[0]);
pHysiX 17:18c3bd016e49 46
pHysiX 17:18c3bd016e49 47 yaw_adjust = yawPIDrate.compute();
pHysiX 17:18c3bd016e49 48 pitch_adjust = pitchPIDrate.compute();
pHysiX 17:18c3bd016e49 49 roll_adjust = rollPIDrate.compute();
pHysiX 17:18c3bd016e49 50 } else {
pHysiX 18:af657c4c3944 51 yawPIDrate.setSetPoint(0);
pHysiX 18:af657c4c3944 52 pitchPIDrate.setSetPoint(0);
pHysiX 18:af657c4c3944 53 rollPIDrate.setSetPoint(0);
pHysiX 17:18c3bd016e49 54
pHysiX 17:18c3bd016e49 55 imu.getRotation(&gx, &gy, &gz);
pHysiX 17:18c3bd016e49 56 gyro[0] = ((float) gx / 32.8) + 2;
pHysiX 17:18c3bd016e49 57 gyro[1] = ((float) gy / 32.8);
pHysiX 17:18c3bd016e49 58 gyro[2] = ((float) gz / 32.8);
pHysiX 17:18c3bd016e49 59
pHysiX 17:18c3bd016e49 60 yawPIDrate.setProcessValue(gyro[2]);
pHysiX 17:18c3bd016e49 61 pitchPIDrate.setProcessValue(gyro[1]);
pHysiX 17:18c3bd016e49 62 rollPIDrate.setProcessValue(gyro[0]);
pHysiX 17:18c3bd016e49 63
pHysiX 17:18c3bd016e49 64 yaw_adjust = yawPIDrate.compute();
pHysiX 17:18c3bd016e49 65 pitch_adjust = pitchPIDrate.compute();
pHysiX 17:18c3bd016e49 66 roll_adjust = rollPIDrate.compute();
pHysiX 17:18c3bd016e49 67 }
pHysiX 17:18c3bd016e49 68
pHysiX 17:18c3bd016e49 69 */
pHysiX 17:18c3bd016e49 70
pHysiX 17:18c3bd016e49 71
pHysiX 11:f9fd410c48c2 72 imu.getRotation(&gx, &gy, &gz);
pHysiX 10:ef5fe86f67fe 73 gyro[0] = ((float) gx / 32.8) + 2;
pHysiX 10:ef5fe86f67fe 74 gyro[1] = ((float) gy / 32.8);
pHysiX 10:ef5fe86f67fe 75 gyro[2] = ((float) gz / 32.8);
pHysiX 11:f9fd410c48c2 76
pHysiX 9:371950017779 77 yawPIDrate.setSetPoint((RCCommand[0]-1500)*9/100);
pHysiX 9:371950017779 78 pitchPIDrate.setSetPoint((RCCommand[1]-1500)*-1*9/100);
pHysiX 9:371950017779 79 rollPIDrate.setSetPoint((RCCommand[2]-1500)*9/100);
pHysiX 5:4879ef0e6d41 80
pHysiX 10:ef5fe86f67fe 81 yawPIDrate.setProcessValue(gyro[2]);
pHysiX 10:ef5fe86f67fe 82 pitchPIDrate.setProcessValue(gyro[1]);
pHysiX 10:ef5fe86f67fe 83 rollPIDrate.setProcessValue(gyro[0]);
pHysiX 5:4879ef0e6d41 84
pHysiX 3:605fbcb54e75 85 yaw_adjust = yawPIDrate.compute();
pHysiX 3:605fbcb54e75 86 pitch_adjust = pitchPIDrate.compute();
pHysiX 3:605fbcb54e75 87 roll_adjust = rollPIDrate.compute();
pHysiX 5:4879ef0e6d41 88
pHysiX 10:ef5fe86f67fe 89 counterESC = true;
pHysiX 11:f9fd410c48c2 90
pHysiX 10:ef5fe86f67fe 91 if (gyro_out)
pHysiX 11:f9fd410c48c2 92 BT.printf("%4d %4d %4d\n", gyro[0], gyro[1], gyro[2]);
pHysiX 10:ef5fe86f67fe 93
pHysiX 2:ab967d7b4346 94 //LED[2] = !LED[2];
pHysiX 1:43f8ac7ca6d7 95 }