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@51:04c6af4319e1, 2014-05-19 (annotated)

Committer:
pHysiX
Date:
Mon May 19 15:30:05 2014 +0000
Revision:
51:04c6af4319e1
Parent:
50:8a0accb23007 
Code rearranged into threads. Timer reserved for time critical routines only. Implemented working semaphore and mutex

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 36:d95e3d6f2fc4 4 * Purpose: Thread2S: Slave PID control loop (rate)
pHysiX 36:d95e3d6f2fc4 5 * Functions: Gyro sample
pHysiX 36:d95e3d6f2fc4 6 * Settings: 400Hz
pHysiX 30:d9b988f8d84f 7 * Timing: 290us
pHysiX 25:a7cfe421cb4a 8 */
pHysiX 26:4a3323ee36d5 9 #include "Task2_Slave.h"
pHysiX 1:43f8ac7ca6d7 10 #include "setup.h"
pHysiX 3:605fbcb54e75 11 #include "PID.h"
pHysiX 3:605fbcb54e75 12
pHysiX 50:8a0accb23007 13 Semaphore sem_Task2_Slave(1);
pHysiX 50:8a0accb23007 14
pHysiX 21:b642c18eccd1 15 /* YPR Adjust */
pHysiX 21:b642c18eccd1 16 volatile float adjust[3] = {0.0, 0.0, 0.0};
pHysiX 21:b642c18eccd1 17
pHysiX 3:605fbcb54e75 18 int16_t gx, gy, gz;
pHysiX 10:ef5fe86f67fe 19 volatile int gyro[3] = {0, 0, 0};
pHysiX 10:ef5fe86f67fe 20
pHysiX 1:43f8ac7ca6d7 21
pHysiX 34:228d87c45151 22
pHysiX 34:228d87c45151 23
pHysiX 34:228d87c45151 24 // ===============================
pHysiX 34:228d87c45151 25 // === GYRO SAMPLE & SLAVE PID ===
pHysiX 34:228d87c45151 26 // ===============================
pHysiX 27:18b6580eb0b1 27 void Task2_Slave(void const *argument)
pHysiX 1:43f8ac7ca6d7 28 {
pHysiX 50:8a0accb23007 29 while (1) {
pHysiX 51:04c6af4319e1 30 //PC.printf("T2S\n");
pHysiX 50:8a0accb23007 31 sem_Task2_Slave.wait();
pHysiX 51:04c6af4319e1 32 //PC.printf("T2S Sem\n");
pHysiX 51:04c6af4319e1 33
pHysiX 51:04c6af4319e1 34 mutex_i2c.lock();
pHysiX 50:8a0accb23007 35 gyroSample();
pHysiX 51:04c6af4319e1 36 mutex_i2c.unlock();
pHysiX 17:18c3bd016e49 37
pHysiX 50:8a0accb23007 38 if (armed) {
pHysiX 50:8a0accb23007 39 yawPIDrate.setSetPoint(inputYPR[0]);
pHysiX 25:a7cfe421cb4a 40
pHysiX 50:8a0accb23007 41 switch (mode) {
pHysiX 50:8a0accb23007 42 case RATE:
pHysiX 50:8a0accb23007 43 pitchPIDrate.setSetPoint(inputYPR[1]);
pHysiX 50:8a0accb23007 44 rollPIDrate.setSetPoint(inputYPR[2]);
pHysiX 17:18c3bd016e49 45
pHysiX 50:8a0accb23007 46 yawPIDrate.setProcessValue(gyro[2]);
pHysiX 50:8a0accb23007 47 pitchPIDrate.setProcessValue(gyro[1]);
pHysiX 50:8a0accb23007 48 rollPIDrate.setProcessValue(gyro[0]);
pHysiX 30:d9b988f8d84f 49
pHysiX 50:8a0accb23007 50 adjust[0] = yawPIDrate.compute();
pHysiX 50:8a0accb23007 51 adjust[1] = pitchPIDrate.compute();
pHysiX 50:8a0accb23007 52 adjust[2] = rollPIDrate.compute();
pHysiX 51:04c6af4319e1 53
pHysiX 50:8a0accb23007 54 break;
pHysiX 25:a7cfe421cb4a 55
pHysiX 50:8a0accb23007 56 case ATTITUDE:
pHysiX 50:8a0accb23007 57 default:
pHysiX 50:8a0accb23007 58 pitchPIDrate.setSetPoint(adjust_attitude[1]);
pHysiX 50:8a0accb23007 59 rollPIDrate.setSetPoint(adjust_attitude[2]);
pHysiX 5:4879ef0e6d41 60
pHysiX 50:8a0accb23007 61 yawPIDrate.setProcessValue(gyro[2]);
pHysiX 50:8a0accb23007 62 pitchPIDrate.setProcessValue(gyro[1]);
pHysiX 50:8a0accb23007 63 rollPIDrate.setProcessValue(gyro[0]);
pHysiX 5:4879ef0e6d41 64
pHysiX 50:8a0accb23007 65 adjust[0] = yawPIDrate.compute();
pHysiX 50:8a0accb23007 66 adjust[1] = pitchPIDrate.compute();
pHysiX 50:8a0accb23007 67 adjust[2] = rollPIDrate.compute();
pHysiX 21:b642c18eccd1 68
pHysiX 50:8a0accb23007 69 break;
pHysiX 50:8a0accb23007 70 }
pHysiX 51:04c6af4319e1 71 sem_Task4.release();
pHysiX 30:d9b988f8d84f 72 }
pHysiX 51:04c6af4319e1 73 Thread::wait(TASK2_SLAVE_PERIOD);
pHysiX 25:a7cfe421cb4a 74 }
pHysiX 21:b642c18eccd1 75 }
pHysiX 33:f88a6ee18103 76
pHysiX 36:d95e3d6f2fc4 77
pHysiX 36:d95e3d6f2fc4 78
pHysiX 36:d95e3d6f2fc4 79
pHysiX 34:228d87c45151 80 // ************************
pHysiX 34:228d87c45151 81 // *** Helper functions ***
pHysiX 34:228d87c45151 82 // ************************
pHysiX 50:8a0accb23007 83 void Task2_Slave_ISR(void const *argument)
pHysiX 50:8a0accb23007 84 {
pHysiX 51:04c6af4319e1 85 //PC.printf("T2S ISR\n");
pHysiX 50:8a0accb23007 86 sem_Task2_Slave.release();
pHysiX 50:8a0accb23007 87 }
pHysiX 50:8a0accb23007 88
pHysiX 36:d95e3d6f2fc4 89 void gyroSample(void)
pHysiX 36:d95e3d6f2fc4 90 {
pHysiX 48:9dbdc4144f00 91 imu_nb.getRotation(&gx, &gy, &gz);
pHysiX 36:d95e3d6f2fc4 92 gyro[0] = gx + 60;
pHysiX 36:d95e3d6f2fc4 93 gyro[1] = gy - 15;
pHysiX 38:ef65533cca32 94 gyro[2] = gz - 8;
pHysiX 36:d95e3d6f2fc4 95
pHysiX 36:d95e3d6f2fc4 96 for (int i = 0; i < 3; i++)
pHysiX 36:d95e3d6f2fc4 97 gyro[i] /= (float) 32.8;
pHysiX 36:d95e3d6f2fc4 98 }