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:
9:371950017779
Parent:
7:3d28cfc4901b
Child:
10:ef5fe86f67fe

File content as of revision 9:371950017779:

/* PID (100Hz) */

#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/100);
    pitchPIDrate.setSetPoint((RCCommand[1]-1500)*-1*9/100);
    rollPIDrate.setSetPoint((RCCommand[2]-1500)*9/100);

    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];
}