File content as of revision 20:b193a50a2ba3:

/* RC & BT Command & Telemetry (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};
AnalogIn voltageSense(p20);

float vIn = 0.0;

/* [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 == '0' || data == '9') {
            box_demo = false;
            rc_out = false;
            gyro_out = false;

            yawPIDrate.reset();
            pitchPIDrate.reset();
            rollPIDrate.reset();
        } else 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_use[0];
            ypr_offset[1] = ypr_use[1];
            ypr_offset[2] = ypr_use[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 == '1') {
            box_demo = false;
            rc_out = false;
            gyro_out = false;
            KP_YAW_RATE += 0.1;
            BT.printf("KP Y Rate: %3.4f\n", KP_YAW_RATE);
        } else if (data == '2') {
            box_demo = false;
            rc_out = false;
            gyro_out = false;
            KP_PITCH_RATE += 0.1;
            BT.printf("KP P Rate: %3.4f\n", KP_PITCH_RATE);
        } else if (data == '3') {
            box_demo = false;
            rc_out = false;
            gyro_out = false;
            KP_ROLL_RATE += 0.1;
            BT.printf("KP R Rate: %3.4f\n", KP_ROLL_RATE);
        }
    }

    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;
    } else {
        for (int i = 0; i < 5; i++)
            RCCommand[i] = constrainRCCommand(RCCommand[i]);
    }

    if (box_demo) {
        BT.printf("\nV%2.2f\n", voltageSense*VOLTAGE_SCALE);
        BT.printf("\nA%3.2f\nT%2.2f\n", altimeter.Altitude_m(), altimeter.Temp_C());
    } else 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];
}

int constrainRCCommand(int input)
{
    if (input < 1000)
        return 1000;
    else if (input > 2000)
        return 2000;
    else
        return input;
}