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

Committer:
pHysiX
Date:
Wed Apr 30 13:49:12 2014 +0000
Revision:
9:371950017779
Parent:
7:3d28cfc4901b
Child:
10:ef5fe86f67fe
Task1: Fix FIFO, or change to raw mode; Task2: Fix PID motor outputs; Task3: Need disarm/arm, calibration; Task4: Check motor mixing

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 9:371950017779 15 yawPIDrate.setSetPoint((RCCommand[0]-1500)*9/100);
pHysiX 9:371950017779 16 pitchPIDrate.setSetPoint((RCCommand[1]-1500)*-1*9/100);
pHysiX 9:371950017779 17 rollPIDrate.setSetPoint((RCCommand[2]-1500)*9/100);
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 }