File content as of revision 12:953d25061417:

/* 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 == '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 (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];
}