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@7:3d28cfc4901b, 2014-04-30 (annotated)

Committer:
pHysiX
Date:
Wed Apr 30 07:17:17 2014 +0000
Revision:
7:3d28cfc4901b
Parent:
5:4879ef0e6d41
Child:
9:371950017779
Version 1.0: All systems implemented. Need to test RTOS stability, ESC behaviour during RTOS stall, and tune PID.; ; Need to implement PID based on Kp Ki Kd instead of Kp Ti Td.

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 1:43f8ac7ca6d7 12
pHysiX 1:43f8ac7ca6d7 13 void Task2(void const *argument)
pHysiX 1:43f8ac7ca6d7 14 {
pHysiX 5:4879ef0e6d41 15 yawPIDrate.setSetPoint((RCCommand[0]-1500)*9/10);
pHysiX 5:4879ef0e6d41 16 pitchPIDrate.setSetPoint((RCCommand[1]-1500)*-1*9/10);
pHysiX 5:4879ef0e6d41 17 rollPIDrate.setSetPoint((RCCommand[2]-1500)*9/10);
pHysiX 5:4879ef0e6d41 18
pHysiX 3:605fbcb54e75 19 imu.getRotation(&gx, &gy, &gz);
pHysiX 3:605fbcb54e75 20 gx /= 32.8;
pHysiX 5:4879ef0e6d41 21 gx += 2;
pHysiX 3:605fbcb54e75 22 gy /= 32.8;
pHysiX 3:605fbcb54e75 23 gz /= 32.8;
pHysiX 5:4879ef0e6d41 24
pHysiX 5:4879ef0e6d41 25 if (gyro_out)
pHysiX 5:4879ef0e6d41 26 BT.printf("%4d %4d %4d\n", gx, gy, gz);
pHysiX 5:4879ef0e6d41 27
pHysiX 3:605fbcb54e75 28 yawPIDrate.setProcessValue(gz);
pHysiX 3:605fbcb54e75 29 pitchPIDrate.setProcessValue(gy);
pHysiX 5:4879ef0e6d41 30 rollPIDrate.setProcessValue(gx);
pHysiX 5:4879ef0e6d41 31
pHysiX 3:605fbcb54e75 32 yaw_adjust = yawPIDrate.compute();
pHysiX 3:605fbcb54e75 33 pitch_adjust = pitchPIDrate.compute();
pHysiX 3:605fbcb54e75 34 roll_adjust = rollPIDrate.compute();
pHysiX 5:4879ef0e6d41 35
pHysiX 2:ab967d7b4346 36 //LED[2] = !LED[2];
pHysiX 1:43f8ac7ca6d7 37 }