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-19
Revision:
50:8a0accb23007
Parent:
48:9dbdc4144f00
Child:
51:04c6af4319e1

File content as of revision 50:8a0accb23007:

/* File:        Task2_Slave.cpp
 * Author:      Trung Tin Ian HUA
 * Date:        May 2014
 * Purpose:     Thread2S: Slave PID control loop (rate)
 * Functions:   Gyro sample
 * Settings:    400Hz
 * Timing:      290us
 */
#include "Task2_Slave.h"
#include "setup.h"
#include "PID.h"

Semaphore sem_Task2_Slave(1);

/* 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;



// ===============================
// === GYRO SAMPLE & SLAVE PID ===
// ===============================
void Task2_Slave(void const *argument)
{
    while (1) {
        sem_Task2_Slave.wait();
        gyroSample();

        if (armed) {
            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;
            }
            //sem_Task4.release();
        }
    }
}




// ************************
// *** Helper functions ***
// ************************
void Task2_Slave_ISR(void const *argument)
{
    sem_Task2_Slave.release();
}

void gyroSample(void)
{
    imu_nb.getRotation(&gx, &gy, &gz);
    gyro[0] = gx + 60;
    gyro[1] = gy - 15;
    gyro[2] = gz - 8;

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