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_Master.cpp@34:228d87c45151, 2014-05-12 (annotated)

Committer:
pHysiX
Date:
Mon May 12 13:55:34 2014 +0000
Revision:
34:228d87c45151
Parent:
33:f88a6ee18103
Child:
36:d95e3d6f2fc4
Code tidied

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pHysiX 27:18b6580eb0b1 1 /* File: Task2_Master.cpp
pHysiX 27:18b6580eb0b1 2 * Author: Trung Tin Ian HUA
pHysiX 27:18b6580eb0b1 3 * Date: May 2014
pHysiX 31:3dde2201e54d 4 * Purpose: Thread2M: Master PID control loop (attitude)
pHysiX 30:d9b988f8d84f 5 * Settings: 100Hz
pHysiX 30:d9b988f8d84f 6 * Timing: 40us
pHysiX 27:18b6580eb0b1 7 */
pHysiX 27:18b6580eb0b1 8 #include "Task2_Slave.h"
pHysiX 27:18b6580eb0b1 9 #include "setup.h"
pHysiX 27:18b6580eb0b1 10 #include "PID.h"
pHysiX 27:18b6580eb0b1 11
pHysiX 27:18b6580eb0b1 12 /* YPR Adjust */
pHysiX 27:18b6580eb0b1 13 volatile float adjust_attitude[3] = {0.0, 0.0, 0.0};
pHysiX 27:18b6580eb0b1 14
pHysiX 34:228d87c45151 15
pHysiX 34:228d87c45151 16
pHysiX 34:228d87c45151 17 // ===============================
pHysiX 34:228d87c45151 18 // === YPR SAMPLE & MASTER PID ===
pHysiX 34:228d87c45151 19 // ===============================
pHysiX 31:3dde2201e54d 20 //Timer
pHysiX 27:18b6580eb0b1 21 void Task2_Master(void const *argument)
pHysiX 27:18b6580eb0b1 22 {
pHysiX 31:3dde2201e54d 23 //Timer
pHysiX 30:d9b988f8d84f 24 if (armed) {
pHysiX 30:d9b988f8d84f 25 switch (mode) {
pHysiX 30:d9b988f8d84f 26 case RATE:
pHysiX 30:d9b988f8d84f 27 break;
pHysiX 28:aa72bd4ff103 28
pHysiX 30:d9b988f8d84f 29 case ATTITUDE:
pHysiX 30:d9b988f8d84f 30 default:
pHysiX 31:3dde2201e54d 31 //Timer
pHysiX 31:3dde2201e54d 32 AHRSSample();
pHysiX 30:d9b988f8d84f 33
pHysiX 31:3dde2201e54d 34 pitchPIDstable.setProcessValue((int) (ypr[1] - ypr_offset[1]));
pHysiX 31:3dde2201e54d 35 rollPIDstable.setProcessValue((int) (ypr[2] - ypr_offset[2]));
pHysiX 27:18b6580eb0b1 36
pHysiX 30:d9b988f8d84f 37 pitchPIDstable.setSetPoint(inputYPR[1]);
pHysiX 30:d9b988f8d84f 38 rollPIDstable.setSetPoint(inputYPR[2]);
pHysiX 30:d9b988f8d84f 39
pHysiX 30:d9b988f8d84f 40 adjust_attitude[1] = pitchPIDstable.compute();
pHysiX 30:d9b988f8d84f 41 adjust_attitude[2] = rollPIDstable.compute();
pHysiX 30:d9b988f8d84f 42 adjust_attitude[2] *= -1;
pHysiX 27:18b6580eb0b1 43
pHysiX 30:d9b988f8d84f 44 //counterTask2Master = true;
pHysiX 31:3dde2201e54d 45 //Timer
pHysiX 30:d9b988f8d84f 46 break;
pHysiX 30:d9b988f8d84f 47 }
pHysiX 31:3dde2201e54d 48 //Timer
pHysiX 27:18b6580eb0b1 49 }
pHysiX 27:18b6580eb0b1 50 }
pHysiX 33:f88a6ee18103 51
pHysiX 34:228d87c45151 52 // ************************
pHysiX 34:228d87c45151 53 // *** Helper functions ***
pHysiX 34:228d87c45151 54 // ************************