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/Task3.cpp

Committer:
pHysiX
Date:
2014-04-30
Revision:
5:4879ef0e6d41
Parent:
4:01921a136f58
Child:
6:1a5654c14b5b

File content as of revision 5:4879ef0e6d41:

/* RC Command (50Hz) */

#include "tasks.h"
#include "setup.h"
#include "PwmIn.h"

float ypr_offset[3];
bool box_demo = false;
bool rc_out = false;
bool gyro_out = false;

PwmIn rxModule[] = {p14, p15, p16, p17, p18};

/* [YAW PITCH ROLL THROTTLE AUX] */
int RCCommand[5] = {0, 0, 0, 0, 0};

void Task3(void const *argument)
{
    if (BT.readable()) {
        char data = BT.getc();
        if (data == 'B') {
            box_demo = true;
            rc_out = false;
            gyro_out = false;
        } else if (data == 'Z') {
            box_demo = true;
            rc_out = false;
            gyro_out = false;
            ypr_offset[0] = ypr[0];
            ypr_offset[1] = ypr[1];
            ypr_offset[2] = ypr[2];
        } else if (data == 'R') {
            box_demo = false;
            rc_out = true;
            gyro_out = false;
        } else if (data == 'G') {
            box_demo = false;
            rc_out = false;
            gyro_out = true;
        } else if (data == '0') {
            box_demo = false;
            rc_out = false;
            gyro_out = false;
        }
    }
    RCCommand[2] = rxModule[0].pulsewidth(); // Roll
    RCCommand[3] = rxModule[1].pulsewidth(); // Throttle
    RCCommand[1] = rxModule[2].pulsewidth(); // Pitch
    RCCommand[0] = rxModule[3].pulsewidth(); // Yaw
    RCCommand[4] = rxModule[4].pulsewidth(); // AUX

    if (rxModule[1].stallTimer.read_us() > 18820) {
        for (int i = 0; i < 5; i++)
            RCCommand[i] = 0;
    }

    if (rc_out)
        BT.printf("%5d %5d %5d %5d %5d\n", RCCommand[0], RCCommand[1], RCCommand[2], RCCommand[3], RCCommand[4]);

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