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-05-02
Revision:
11:f9fd410c48c2
Parent:
10:ef5fe86f67fe
Child:
12:953d25061417

File content as of revision 11:f9fd410c48c2:

/* 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;
volatile int gyro[3] = {0, 0, 0};

bool counterESC = false;

void Task2(void const *argument)
{
    imu.getRotation(&gx, &gy, &gz);
    gyro[0] = ((float) gx / 32.8) + 2;
    gyro[1] = ((float) gy / 32.8);
    gyro[2] = ((float) gz / 32.8);

    yawPIDrate.setSetPoint((RCCommand[0]-1500)*9/100);
    pitchPIDrate.setSetPoint((RCCommand[1]-1500)*-1*9/100);
    rollPIDrate.setSetPoint((RCCommand[2]-1500)*9/100);

    yawPIDrate.setProcessValue(gyro[2]);
    pitchPIDrate.setProcessValue(gyro[1]);
    rollPIDrate.setProcessValue(gyro[0]);

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

    /*
    if (counterTask1) {
    yawPIDstable.setSetPoint((RCCommand[0]-1500)*9/100);
    pitchPIDstable.setSetPoint(RCCommand[1]-1500)*9/100);
    rollPIDstable.setSetPoint(RCCommand[2]-1500)*9/100);

    yawPIDstable.setProcessValue(ypr[0]);
    pitchPIDstable.setProcessValue(ypr[1]);
    rollPIDstable.setProcessValue(ypr[2]);

    feed into ratePID();

    counterTask1 = false;
    } else {
        rate as above
        }
    */

    counterESC = true;

    if (gyro_out)
        BT.printf("%4d %4d %4d\n", gyro[0], gyro[1], gyro[2]);

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