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

Committer:
pHysiX
Date:
Thu May 08 09:39:12 2014 +0000
Revision:
21:b642c18eccd1
Parent:
18:af657c4c3944
Child:
22:ef8aa9728013
Primary PID disabled. Secondary (rate) PID enabled. P gain tuned

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 21:b642c18eccd1 7 /* YPR Adjust */
pHysiX 21:b642c18eccd1 8 volatile float adjust[3] = {0.0, 0.0, 0.0};
pHysiX 21:b642c18eccd1 9 volatile float adjust_stable[3] = {0.0, 0.0, 0.0};
pHysiX 21:b642c18eccd1 10
pHysiX 3:605fbcb54e75 11
pHysiX 3:605fbcb54e75 12 int16_t gx, gy, gz;
pHysiX 10:ef5fe86f67fe 13 volatile int gyro[3] = {0, 0, 0};
pHysiX 21:b642c18eccd1 14 //volatile int gyro_pid[3] = {0, 0, 0};
pHysiX 10:ef5fe86f67fe 15
pHysiX 10:ef5fe86f67fe 16 bool counterESC = false;
pHysiX 1:43f8ac7ca6d7 17
pHysiX 1:43f8ac7ca6d7 18 void Task2(void const *argument)
pHysiX 1:43f8ac7ca6d7 19 {
pHysiX 21:b642c18eccd1 20 imu.getRotation(&gx, &gy, &gz);
pHysiX 21:b642c18eccd1 21 gyro[0] = gx + 60;
pHysiX 21:b642c18eccd1 22 gyro[1] = gy - 15;
pHysiX 21:b642c18eccd1 23 gyro[2] = gz + 25;
pHysiX 17:18c3bd016e49 24
pHysiX 21:b642c18eccd1 25 for (int i = 0; i < 3; i++)
pHysiX 21:b642c18eccd1 26 gyro[i] /= (float) 32.8;
pHysiX 17:18c3bd016e49 27
pHysiX 21:b642c18eccd1 28 //if (counterTask1) {
pHysiX 21:b642c18eccd1 29 pitchPIDstable.setSetPoint(inputYPR[1]);
pHysiX 21:b642c18eccd1 30 rollPIDstable.setSetPoint(inputYPR[2]);
pHysiX 17:18c3bd016e49 31
pHysiX 21:b642c18eccd1 32 pitchPIDstable.setProcessValue(ypr[1] - ypr_offset[1]);
pHysiX 21:b642c18eccd1 33 rollPIDstable.setProcessValue(ypr[2] - ypr_offset[2]);
pHysiX 17:18c3bd016e49 34
pHysiX 21:b642c18eccd1 35 adjust_stable[1] = pitchPIDstable.compute();
pHysiX 21:b642c18eccd1 36 adjust_stable[2] = rollPIDstable.compute();
pHysiX 17:18c3bd016e49 37
pHysiX 21:b642c18eccd1 38 yawPIDrate.setSetPoint(inputYPR[0]);
pHysiX 21:b642c18eccd1 39 pitchPIDrate.setSetPoint(inputYPR[1]);
pHysiX 21:b642c18eccd1 40 rollPIDrate.setSetPoint(inputYPR[2]);
pHysiX 21:b642c18eccd1 41 //pitchPIDrate.setSetPoint(adjust_stable[1]);
pHysiX 21:b642c18eccd1 42 //rollPIDrate.setSetPoint(adjust_stable[2]);
pHysiX 5:4879ef0e6d41 43
pHysiX 10:ef5fe86f67fe 44 yawPIDrate.setProcessValue(gyro[2]);
pHysiX 10:ef5fe86f67fe 45 pitchPIDrate.setProcessValue(gyro[1]);
pHysiX 10:ef5fe86f67fe 46 rollPIDrate.setProcessValue(gyro[0]);
pHysiX 5:4879ef0e6d41 47
pHysiX 21:b642c18eccd1 48 adjust[0] = yawPIDrate.compute();
pHysiX 21:b642c18eccd1 49 adjust[1] = pitchPIDrate.compute();
pHysiX 21:b642c18eccd1 50 adjust[2] = rollPIDrate.compute();
pHysiX 21:b642c18eccd1 51
pHysiX 21:b642c18eccd1 52 counterTask1 = false;
pHysiX 5:4879ef0e6d41 53
pHysiX 10:ef5fe86f67fe 54 counterESC = true;
pHysiX 11:f9fd410c48c2 55
pHysiX 21:b642c18eccd1 56 //if (adjust_check)
pHysiX 21:b642c18eccd1 57 // BT.printf("%4.4f %4.4f %4.4f\n", yaw_adjust, pitch_adjust, roll_adjust);
pHysiX 21:b642c18eccd1 58
pHysiX 21:b642c18eccd1 59 if (adjust_check)
pHysiX 21:b642c18eccd1 60 BT.printf("%3.4f %3.4f %3.4f\n", adjust[0], adjust[1], adjust[2]);
pHysiX 21:b642c18eccd1 61
pHysiX 10:ef5fe86f67fe 62 if (gyro_out)
pHysiX 21:b642c18eccd1 63 BT.printf("%3d %3d %3d\n", (int) gyro[0], (int) gyro[1], (int) gyro[2]);
pHysiX 21:b642c18eccd1 64 //BT.printf("%3.4f %3.4f %3.4f\n", gyro[0], gyro[1], gyro[2]);
pHysiX 21:b642c18eccd1 65 //BT.printf("%6d %6d %6d\n", gx, gy, gz);
pHysiX 10:ef5fe86f67fe 66
pHysiX 2:ab967d7b4346 67 //LED[2] = !LED[2];
pHysiX 21:b642c18eccd1 68 }