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_Slave.cpp

Committer:
pHysiX
Date:
2014-05-12
Revision:
30:d9b988f8d84f
Parent:
27:18b6580eb0b1
Child:
31:3dde2201e54d

File content as of revision 30:d9b988f8d84f:

/* File:        Task2_Slave.cpp
 * Author:      Trung Tin Ian HUA
 * Date:        May 2014
 * Purpose:     Thread2: Gyro sample and PID Control loop
 * Settings:    200Hz
 * Timing:      290us
 */
#include "Task2_Slave.h"
#include "setup.h"
#include "PID.h"

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

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

bool counterESC = false;

#ifdef TIME_TASK2S
Timer _t2s;
#endif
void Task2_Slave(void const *argument)
{
#ifdef TIME_TASK2S
    _t2s.reset();
    _t2s.start();
#endif
    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;

#ifndef TIME_TASK2S
    if (armed) {
#endif
        yawPIDrate.setSetPoint(inputYPR[0]);

        switch (mode) {
            case RATE:
                pitchPIDrate.setSetPoint(inputYPR[1]);
                rollPIDrate.setSetPoint(inputYPR[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();

                counterESC = true;
                break;

            case ATTITUDE:
            default:
                pitchPIDrate.setSetPoint(adjust_attitude[1]);
                rollPIDrate.setSetPoint(adjust_attitude[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();

                counterESC = true;
                break;
        }
#ifndef TIME_TASK2S
    }
#endif

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

#ifdef TIME_TASK2S
    _t2s.stop();
    BT.printf("%d\n", _t2s.read_us());
#endif
}