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

Committer:
pHysiX
Date:
Thu May 08 13:35:13 2014 +0000
Revision:
25:a7cfe421cb4a
Parent:
24:54a8cdf17378 
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: Task2.cpp
pHysiX 22:ef8aa9728013 2 * Author: Trung Tin Ian HUA
pHysiX 22:ef8aa9728013 3 * Date: May 2014
pHysiX 22:ef8aa9728013 4 * Purpose: Thread2: Gyro sample and PID Control loop
pHysiX 22:ef8aa9728013 5 * Settings: 200Hz
pHysiX 25:a7cfe421cb4a 6 */
pHysiX 2:ab967d7b4346 7
pHysiX 1:43f8ac7ca6d7 8 #include "Task2.h"
pHysiX 1:43f8ac7ca6d7 9 #include "setup.h"
pHysiX 3:605fbcb54e75 10 #include "PID.h"
pHysiX 3:605fbcb54e75 11
pHysiX 21:b642c18eccd1 12 /* YPR Adjust */
pHysiX 21:b642c18eccd1 13 volatile float adjust[3] = {0.0, 0.0, 0.0};
pHysiX 21:b642c18eccd1 14 volatile float adjust_stable[3] = {0.0, 0.0, 0.0};
pHysiX 21:b642c18eccd1 15
pHysiX 3:605fbcb54e75 16 int16_t gx, gy, gz;
pHysiX 10:ef5fe86f67fe 17 volatile int gyro[3] = {0, 0, 0};
pHysiX 10:ef5fe86f67fe 18
pHysiX 10:ef5fe86f67fe 19 bool counterESC = false;
pHysiX 1:43f8ac7ca6d7 20
pHysiX 1:43f8ac7ca6d7 21 void Task2(void const *argument)
pHysiX 1:43f8ac7ca6d7 22 {
pHysiX 21:b642c18eccd1 23 imu.getRotation(&gx, &gy, &gz);
pHysiX 21:b642c18eccd1 24 gyro[0] = gx + 60;
pHysiX 21:b642c18eccd1 25 gyro[1] = gy - 15;
pHysiX 21:b642c18eccd1 26 gyro[2] = gz + 25;
pHysiX 17:18c3bd016e49 27
pHysiX 21:b642c18eccd1 28 for (int i = 0; i < 3; i++)
pHysiX 21:b642c18eccd1 29 gyro[i] /= (float) 32.8;
pHysiX 17:18c3bd016e49 30
pHysiX 21:b642c18eccd1 31 //if (counterTask1) {
pHysiX 25:a7cfe421cb4a 32
pHysiX 25:a7cfe421cb4a 33 switch (mode) {
pHysiX 25:a7cfe421cb4a 34 case RATE:
pHysiX 25:a7cfe421cb4a 35 yawPIDrate.setSetPoint(inputYPR[0]);
pHysiX 25:a7cfe421cb4a 36 pitchPIDrate.setSetPoint(inputYPR[1]);
pHysiX 25:a7cfe421cb4a 37 rollPIDrate.setSetPoint(inputYPR[2]);
pHysiX 25:a7cfe421cb4a 38
pHysiX 25:a7cfe421cb4a 39 yawPIDrate.setProcessValue(gyro[2]);
pHysiX 25:a7cfe421cb4a 40 pitchPIDrate.setProcessValue(gyro[1]);
pHysiX 25:a7cfe421cb4a 41 rollPIDrate.setProcessValue(gyro[0]);
pHysiX 17:18c3bd016e49 42
pHysiX 25:a7cfe421cb4a 43 adjust[0] = yawPIDrate.compute();
pHysiX 25:a7cfe421cb4a 44 adjust[1] = pitchPIDrate.compute();
pHysiX 25:a7cfe421cb4a 45 adjust[2] = rollPIDrate.compute();
pHysiX 25:a7cfe421cb4a 46
pHysiX 25:a7cfe421cb4a 47 counterTask1 = false;
pHysiX 25:a7cfe421cb4a 48 counterESC = true;
pHysiX 17:18c3bd016e49 49
pHysiX 25:a7cfe421cb4a 50 break;
pHysiX 25:a7cfe421cb4a 51
pHysiX 25:a7cfe421cb4a 52 case STABLE:
pHysiX 25:a7cfe421cb4a 53 default:
pHysiX 25:a7cfe421cb4a 54 pitchPIDstable.setSetPoint(inputYPR[1]);
pHysiX 25:a7cfe421cb4a 55 rollPIDstable.setSetPoint(inputYPR[2]);
pHysiX 17:18c3bd016e49 56
pHysiX 25:a7cfe421cb4a 57 pitchPIDstable.setProcessValue((int) (ypr[1] - ypr_offset[1]));
pHysiX 25:a7cfe421cb4a 58 rollPIDstable.setProcessValue((int) (ypr[2] - ypr_offset[2]));
pHysiX 25:a7cfe421cb4a 59
pHysiX 25:a7cfe421cb4a 60 adjust_stable[1] = pitchPIDstable.compute();
pHysiX 25:a7cfe421cb4a 61 adjust_stable[2] = rollPIDstable.compute();
pHysiX 25:a7cfe421cb4a 62 adjust_stable[2] *= -1;
pHysiX 25:a7cfe421cb4a 63
pHysiX 25:a7cfe421cb4a 64 yawPIDrate.setSetPoint(inputYPR[0]);
pHysiX 25:a7cfe421cb4a 65 pitchPIDrate.setSetPoint(adjust_stable[1]);
pHysiX 25:a7cfe421cb4a 66 rollPIDrate.setSetPoint(adjust_stable[2]);
pHysiX 5:4879ef0e6d41 67
pHysiX 25:a7cfe421cb4a 68 yawPIDrate.setProcessValue(gyro[2]);
pHysiX 25:a7cfe421cb4a 69 pitchPIDrate.setProcessValue(gyro[1]);
pHysiX 25:a7cfe421cb4a 70 rollPIDrate.setProcessValue(gyro[0]);
pHysiX 5:4879ef0e6d41 71
pHysiX 25:a7cfe421cb4a 72 adjust[0] = yawPIDrate.compute();
pHysiX 25:a7cfe421cb4a 73 adjust[1] = pitchPIDrate.compute();
pHysiX 25:a7cfe421cb4a 74 adjust[2] = rollPIDrate.compute();
pHysiX 21:b642c18eccd1 75
pHysiX 25:a7cfe421cb4a 76 counterTask1 = false;
pHysiX 25:a7cfe421cb4a 77 counterESC = true;
pHysiX 25:a7cfe421cb4a 78
pHysiX 25:a7cfe421cb4a 79 break;
pHysiX 25:a7cfe421cb4a 80 }
pHysiX 11:f9fd410c48c2 81
pHysiX 21:b642c18eccd1 82 if (adjust_check)
pHysiX 21:b642c18eccd1 83 BT.printf("%3.4f %3.4f %3.4f\n", adjust[0], adjust[1], adjust[2]);
pHysiX 21:b642c18eccd1 84
pHysiX 10:ef5fe86f67fe 85 if (gyro_out)
pHysiX 21:b642c18eccd1 86 BT.printf("%3d %3d %3d\n", (int) gyro[0], (int) gyro[1], (int) gyro[2]);
pHysiX 21:b642c18eccd1 87 }