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_Slave.cpp@30:d9b988f8d84f, 2014-05-12 (annotated)

Committer:
pHysiX
Date:
Mon May 12 04:43:38 2014 +0000
Revision:
30:d9b988f8d84f
Parent:
27:18b6580eb0b1
Child:
31:3dde2201e54d
WIP

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pHysiX 27:18b6580eb0b1 1 /* File: Task2_Slave.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 30:d9b988f8d84f 6 * Timing: 290us
pHysiX 25:a7cfe421cb4a 7 */
pHysiX 26:4a3323ee36d5 8 #include "Task2_Slave.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
pHysiX 3:605fbcb54e75 15 int16_t gx, gy, gz;
pHysiX 10:ef5fe86f67fe 16 volatile int gyro[3] = {0, 0, 0};
pHysiX 10:ef5fe86f67fe 17
pHysiX 10:ef5fe86f67fe 18 bool counterESC = false;
pHysiX 1:43f8ac7ca6d7 19
pHysiX 30:d9b988f8d84f 20 #ifdef TIME_TASK2S
pHysiX 30:d9b988f8d84f 21 Timer _t2s;
pHysiX 30:d9b988f8d84f 22 #endif
pHysiX 27:18b6580eb0b1 23 void Task2_Slave(void const *argument)
pHysiX 1:43f8ac7ca6d7 24 {
pHysiX 30:d9b988f8d84f 25 #ifdef TIME_TASK2S
pHysiX 30:d9b988f8d84f 26 _t2s.reset();
pHysiX 30:d9b988f8d84f 27 _t2s.start();
pHysiX 30:d9b988f8d84f 28 #endif
pHysiX 21:b642c18eccd1 29 imu.getRotation(&gx, &gy, &gz);
pHysiX 21:b642c18eccd1 30 gyro[0] = gx + 60;
pHysiX 21:b642c18eccd1 31 gyro[1] = gy - 15;
pHysiX 21:b642c18eccd1 32 gyro[2] = gz + 25;
pHysiX 17:18c3bd016e49 33
pHysiX 21:b642c18eccd1 34 for (int i = 0; i < 3; i++)
pHysiX 21:b642c18eccd1 35 gyro[i] /= (float) 32.8;
pHysiX 17:18c3bd016e49 36
pHysiX 30:d9b988f8d84f 37 #ifndef TIME_TASK2S
pHysiX 30:d9b988f8d84f 38 if (armed) {
pHysiX 30:d9b988f8d84f 39 #endif
pHysiX 30:d9b988f8d84f 40 yawPIDrate.setSetPoint(inputYPR[0]);
pHysiX 25:a7cfe421cb4a 41
pHysiX 30:d9b988f8d84f 42 switch (mode) {
pHysiX 30:d9b988f8d84f 43 case RATE:
pHysiX 30:d9b988f8d84f 44 pitchPIDrate.setSetPoint(inputYPR[1]);
pHysiX 30:d9b988f8d84f 45 rollPIDrate.setSetPoint(inputYPR[2]);
pHysiX 17:18c3bd016e49 46
pHysiX 30:d9b988f8d84f 47 yawPIDrate.setProcessValue(gyro[2]);
pHysiX 30:d9b988f8d84f 48 pitchPIDrate.setProcessValue(gyro[1]);
pHysiX 30:d9b988f8d84f 49 rollPIDrate.setProcessValue(gyro[0]);
pHysiX 30:d9b988f8d84f 50
pHysiX 30:d9b988f8d84f 51 adjust[0] = yawPIDrate.compute();
pHysiX 30:d9b988f8d84f 52 adjust[1] = pitchPIDrate.compute();
pHysiX 30:d9b988f8d84f 53 adjust[2] = rollPIDrate.compute();
pHysiX 25:a7cfe421cb4a 54
pHysiX 30:d9b988f8d84f 55 counterESC = true;
pHysiX 30:d9b988f8d84f 56 break;
pHysiX 25:a7cfe421cb4a 57
pHysiX 30:d9b988f8d84f 58 case ATTITUDE:
pHysiX 30:d9b988f8d84f 59 default:
pHysiX 30:d9b988f8d84f 60 pitchPIDrate.setSetPoint(adjust_attitude[1]);
pHysiX 30:d9b988f8d84f 61 rollPIDrate.setSetPoint(adjust_attitude[2]);
pHysiX 5:4879ef0e6d41 62
pHysiX 30:d9b988f8d84f 63 yawPIDrate.setProcessValue(gyro[2]);
pHysiX 30:d9b988f8d84f 64 pitchPIDrate.setProcessValue(gyro[1]);
pHysiX 30:d9b988f8d84f 65 rollPIDrate.setProcessValue(gyro[0]);
pHysiX 5:4879ef0e6d41 66
pHysiX 30:d9b988f8d84f 67 adjust[0] = yawPIDrate.compute();
pHysiX 30:d9b988f8d84f 68 adjust[1] = pitchPIDrate.compute();
pHysiX 30:d9b988f8d84f 69 adjust[2] = rollPIDrate.compute();
pHysiX 21:b642c18eccd1 70
pHysiX 30:d9b988f8d84f 71 counterESC = true;
pHysiX 30:d9b988f8d84f 72 break;
pHysiX 30:d9b988f8d84f 73 }
pHysiX 30:d9b988f8d84f 74 #ifndef TIME_TASK2S
pHysiX 25:a7cfe421cb4a 75 }
pHysiX 30:d9b988f8d84f 76 #endif
pHysiX 11:f9fd410c48c2 77
pHysiX 21:b642c18eccd1 78 if (adjust_check)
pHysiX 21:b642c18eccd1 79 BT.printf("%3.4f %3.4f %3.4f\n", adjust[0], adjust[1], adjust[2]);
pHysiX 21:b642c18eccd1 80
pHysiX 10:ef5fe86f67fe 81 if (gyro_out)
pHysiX 21:b642c18eccd1 82 BT.printf("%3d %3d %3d\n", (int) gyro[0], (int) gyro[1], (int) gyro[2]);
pHysiX 30:d9b988f8d84f 83
pHysiX 30:d9b988f8d84f 84 #ifdef TIME_TASK2S
pHysiX 30:d9b988f8d84f 85 _t2s.stop();
pHysiX 30:d9b988f8d84f 86 BT.printf("%d\n", _t2s.read_us());
pHysiX 30:d9b988f8d84f 87 #endif
pHysiX 21:b642c18eccd1 88 }