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

Committer:
pHysiX
Date:
2014-04-30
Revision:
5:4879ef0e6d41
Parent:
3:605fbcb54e75
Child:
7:3d28cfc4901b

File content as of revision 5:4879ef0e6d41:

/* PID */

#include "Task2.h"
#include "setup.h"
#include "PID.h"

int yaw_adjust = 0;
int pitch_adjust = 0;
int roll_adjust = 0;

int16_t gx, gy, gz;

void Task2(void const *argument)
{
    yawPIDrate.setSetPoint((RCCommand[0]-1500)*9/10);
    pitchPIDrate.setSetPoint((RCCommand[1]-1500)*-1*9/10);
    rollPIDrate.setSetPoint((RCCommand[2]-1500)*9/10);

    imu.getRotation(&gx, &gy, &gz);
    gx /= 32.8;
    gx += 2;
    gy /= 32.8;
    gz /= 32.8;

    if (gyro_out)
        BT.printf("%4d %4d %4d\n", gx, gy, gz);

    yawPIDrate.setProcessValue(gz);
    pitchPIDrate.setProcessValue(gy);
    rollPIDrate.setProcessValue(gx);

    yaw_adjust = yawPIDrate.compute();
    pitch_adjust = pitchPIDrate.compute();
    roll_adjust = rollPIDrate.compute();

    //LED[2] = !LED[2];
}