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@11:f9fd410c48c2, 2014-05-02 (annotated)

Committer:
pHysiX
Date:
Fri May 02 07:22:09 2014 +0000
Revision:
11:f9fd410c48c2
Parent:
10:ef5fe86f67fe
Child:
12:953d25061417
Fixed quaternion, gyro at 200Hz.; ; Need arming sequence and calibration ability

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pHysiX 7:3d28cfc4901b 1 /* PID (100Hz) */
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 3:605fbcb54e75 7 int yaw_adjust = 0;
pHysiX 3:605fbcb54e75 8 int pitch_adjust = 0;
pHysiX 3:605fbcb54e75 9 int roll_adjust = 0;
pHysiX 3:605fbcb54e75 10
pHysiX 3:605fbcb54e75 11 int16_t gx, gy, gz;
pHysiX 10:ef5fe86f67fe 12 volatile int gyro[3] = {0, 0, 0};
pHysiX 10:ef5fe86f67fe 13
pHysiX 10:ef5fe86f67fe 14 bool counterESC = false;
pHysiX 1:43f8ac7ca6d7 15
pHysiX 1:43f8ac7ca6d7 16 void Task2(void const *argument)
pHysiX 1:43f8ac7ca6d7 17 {
pHysiX 11:f9fd410c48c2 18 imu.getRotation(&gx, &gy, &gz);
pHysiX 10:ef5fe86f67fe 19 gyro[0] = ((float) gx / 32.8) + 2;
pHysiX 10:ef5fe86f67fe 20 gyro[1] = ((float) gy / 32.8);
pHysiX 10:ef5fe86f67fe 21 gyro[2] = ((float) gz / 32.8);
pHysiX 11:f9fd410c48c2 22
pHysiX 9:371950017779 23 yawPIDrate.setSetPoint((RCCommand[0]-1500)*9/100);
pHysiX 9:371950017779 24 pitchPIDrate.setSetPoint((RCCommand[1]-1500)*-1*9/100);
pHysiX 9:371950017779 25 rollPIDrate.setSetPoint((RCCommand[2]-1500)*9/100);
pHysiX 5:4879ef0e6d41 26
pHysiX 10:ef5fe86f67fe 27 yawPIDrate.setProcessValue(gyro[2]);
pHysiX 10:ef5fe86f67fe 28 pitchPIDrate.setProcessValue(gyro[1]);
pHysiX 10:ef5fe86f67fe 29 rollPIDrate.setProcessValue(gyro[0]);
pHysiX 5:4879ef0e6d41 30
pHysiX 3:605fbcb54e75 31 yaw_adjust = yawPIDrate.compute();
pHysiX 3:605fbcb54e75 32 pitch_adjust = pitchPIDrate.compute();
pHysiX 3:605fbcb54e75 33 roll_adjust = rollPIDrate.compute();
pHysiX 5:4879ef0e6d41 34
pHysiX 10:ef5fe86f67fe 35 /*
pHysiX 10:ef5fe86f67fe 36 if (counterTask1) {
pHysiX 10:ef5fe86f67fe 37 yawPIDstable.setSetPoint((RCCommand[0]-1500)*9/100);
pHysiX 10:ef5fe86f67fe 38 pitchPIDstable.setSetPoint(RCCommand[1]-1500)*9/100);
pHysiX 10:ef5fe86f67fe 39 rollPIDstable.setSetPoint(RCCommand[2]-1500)*9/100);
pHysiX 10:ef5fe86f67fe 40
pHysiX 10:ef5fe86f67fe 41 yawPIDstable.setProcessValue(ypr[0]);
pHysiX 10:ef5fe86f67fe 42 pitchPIDstable.setProcessValue(ypr[1]);
pHysiX 10:ef5fe86f67fe 43 rollPIDstable.setProcessValue(ypr[2]);
pHysiX 10:ef5fe86f67fe 44
pHysiX 10:ef5fe86f67fe 45 feed into ratePID();
pHysiX 11:f9fd410c48c2 46
pHysiX 10:ef5fe86f67fe 47 counterTask1 = false;
pHysiX 10:ef5fe86f67fe 48 } else {
pHysiX 10:ef5fe86f67fe 49 rate as above
pHysiX 10:ef5fe86f67fe 50 }
pHysiX 10:ef5fe86f67fe 51 */
pHysiX 10:ef5fe86f67fe 52
pHysiX 10:ef5fe86f67fe 53 counterESC = true;
pHysiX 11:f9fd410c48c2 54
pHysiX 10:ef5fe86f67fe 55 if (gyro_out)
pHysiX 11:f9fd410c48c2 56 BT.printf("%4d %4d %4d\n", gyro[0], gyro[1], gyro[2]);
pHysiX 10:ef5fe86f67fe 57
pHysiX 2:ab967d7b4346 58 //LED[2] = !LED[2];
pHysiX 1:43f8ac7ca6d7 59 }