File content as of revision 21:b642c18eccd1:

/* Gyro & PID (200Hz) */

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

/* YPR Adjust */
volatile float adjust[3] = {0.0, 0.0, 0.0};
volatile float adjust_stable[3] = {0.0, 0.0, 0.0};


int16_t gx, gy, gz;
volatile int gyro[3] = {0, 0, 0};
//volatile int gyro_pid[3] = {0, 0, 0};

bool counterESC = false;

void Task2(void const *argument)
{
    imu.getRotation(&gx, &gy, &gz);
    gyro[0] = gx + 60;
    gyro[1] = gy - 15;
    gyro[2] = gz + 25;

    for (int i = 0; i < 3; i++)
        gyro[i] /= (float) 32.8;

    //if (counterTask1) {
    pitchPIDstable.setSetPoint(inputYPR[1]);
    rollPIDstable.setSetPoint(inputYPR[2]);

    pitchPIDstable.setProcessValue(ypr[1] - ypr_offset[1]);
    rollPIDstable.setProcessValue(ypr[2] - ypr_offset[2]);

    adjust_stable[1] = pitchPIDstable.compute();
    adjust_stable[2] = rollPIDstable.compute();

    yawPIDrate.setSetPoint(inputYPR[0]);
    pitchPIDrate.setSetPoint(inputYPR[1]);
    rollPIDrate.setSetPoint(inputYPR[2]);
    //pitchPIDrate.setSetPoint(adjust_stable[1]);
    //rollPIDrate.setSetPoint(adjust_stable[2]);

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

    adjust[0] = yawPIDrate.compute();
    adjust[1] = pitchPIDrate.compute();
    adjust[2] = rollPIDrate.compute();

    counterTask1 = false;

    counterESC = true;

    //if (adjust_check)
    //  BT.printf("%4.4f %4.4f %4.4f\n", yaw_adjust, pitch_adjust, roll_adjust);

    if (adjust_check)
        BT.printf("%3.4f %3.4f %3.4f\n", adjust[0], adjust[1], adjust[2]);

    if (gyro_out)
        BT.printf("%3d %3d %3d\n", (int) gyro[0], (int) gyro[1], (int) gyro[2]);
    //BT.printf("%3.4f %3.4f %3.4f\n", gyro[0], gyro[1], gyro[2]);
    //BT.printf("%6d %6d %6d\n", gx, gy, gz);

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