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-03
Revision:
18:af657c4c3944
Parent:
17:18c3bd016e49
Child:
21:b642c18eccd1

File content as of revision 18:af657c4c3944:

/* Gyro & PID (200Hz) */

#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)
{

    /*
        if (counterTask1 > 1) {
            //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]);

            counterTask1 = 0;

            pitch_adjust_stable = pitchPIDstable.compute();
            roll_adjust_stable = rollPIDstable.compute();

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

            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.setProcessValue(gyro[2]);
            pitchPIDrate.setProcessValue(gyro[1]);
            rollPIDrate.setProcessValue(gyro[0]);

            yaw_adjust = yawPIDrate.compute();
            pitch_adjust = pitchPIDrate.compute();
            roll_adjust = rollPIDrate.compute();
        } else {
            yawPIDrate.setSetPoint(0);
            pitchPIDrate.setSetPoint(0);
            rollPIDrate.setSetPoint(0);

            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.setProcessValue(gyro[2]);
            pitchPIDrate.setProcessValue(gyro[1]);
            rollPIDrate.setProcessValue(gyro[0]);

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

    */


    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();

    counterESC = true;

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

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