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@12:953d25061417, 2014-05-02 (annotated)

Committer:
pHysiX
Date:
Fri May 02 17:01:56 2014 +0000
Revision:
12:953d25061417
Parent:
11:f9fd410c48c2
Child:
17:18c3bd016e49
Added in all sensors. Need to add in EEPROM to complete control of Tilty. Finished all telemetry output and appropriate data rates.

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 11:f9fd410c48c2 18 imu.getRotation(&gx, &gy, &gz);
pHysiX 10:ef5fe86f67fe 19 gyro[0] = ((float) gx / 32.8) + 2;
pHysiX 10:ef5fe86f67fe 20 gyro[1] = ((float) gy / 32.8);
pHysiX 10:ef5fe86f67fe 21 gyro[2] = ((float) gz / 32.8);
pHysiX 11:f9fd410c48c2 22
pHysiX 9:371950017779 23 yawPIDrate.setSetPoint((RCCommand[0]-1500)*9/100);
pHysiX 9:371950017779 24 pitchPIDrate.setSetPoint((RCCommand[1]-1500)*-1*9/100);
pHysiX 9:371950017779 25 rollPIDrate.setSetPoint((RCCommand[2]-1500)*9/100);
pHysiX 5:4879ef0e6d41 26
pHysiX 10:ef5fe86f67fe 27 yawPIDrate.setProcessValue(gyro[2]);
pHysiX 10:ef5fe86f67fe 28 pitchPIDrate.setProcessValue(gyro[1]);
pHysiX 10:ef5fe86f67fe 29 rollPIDrate.setProcessValue(gyro[0]);
pHysiX 5:4879ef0e6d41 30
pHysiX 3:605fbcb54e75 31 yaw_adjust = yawPIDrate.compute();
pHysiX 3:605fbcb54e75 32 pitch_adjust = pitchPIDrate.compute();
pHysiX 3:605fbcb54e75 33 roll_adjust = rollPIDrate.compute();
pHysiX 5:4879ef0e6d41 34
pHysiX 10:ef5fe86f67fe 35 /*
pHysiX 10:ef5fe86f67fe 36 if (counterTask1) {
pHysiX 10:ef5fe86f67fe 37 yawPIDstable.setSetPoint((RCCommand[0]-1500)*9/100);
pHysiX 10:ef5fe86f67fe 38 pitchPIDstable.setSetPoint(RCCommand[1]-1500)*9/100);
pHysiX 10:ef5fe86f67fe 39 rollPIDstable.setSetPoint(RCCommand[2]-1500)*9/100);
pHysiX 10:ef5fe86f67fe 40
pHysiX 10:ef5fe86f67fe 41 yawPIDstable.setProcessValue(ypr[0]);
pHysiX 10:ef5fe86f67fe 42 pitchPIDstable.setProcessValue(ypr[1]);
pHysiX 10:ef5fe86f67fe 43 rollPIDstable.setProcessValue(ypr[2]);
pHysiX 10:ef5fe86f67fe 44
pHysiX 10:ef5fe86f67fe 45 feed into ratePID();
pHysiX 11:f9fd410c48c2 46
pHysiX 10:ef5fe86f67fe 47 counterTask1 = false;
pHysiX 10:ef5fe86f67fe 48 } else {
pHysiX 10:ef5fe86f67fe 49 rate as above
pHysiX 10:ef5fe86f67fe 50 }
pHysiX 10:ef5fe86f67fe 51 */
pHysiX 10:ef5fe86f67fe 52
pHysiX 10:ef5fe86f67fe 53 counterESC = true;
pHysiX 11:f9fd410c48c2 54
pHysiX 10:ef5fe86f67fe 55 if (gyro_out)
pHysiX 11:f9fd410c48c2 56 BT.printf("%4d %4d %4d\n", gyro[0], gyro[1], gyro[2]);
pHysiX 10:ef5fe86f67fe 57
pHysiX 2:ab967d7b4346 58 //LED[2] = !LED[2];
pHysiX 1:43f8ac7ca6d7 59 }