Subversion Repositories FlightCtrl

#include "externalcontrol.h"
#include "configuration.h"
#include "controlMixer.h"

ExternalControl_t externalControl;
uint8_t externalControlActive;

int16_t EC_PRTY[4];

// TODO: Who is going to call this
void EC_setNeutral(void) {
        // if necessary. From main.c.
        externalControl.config = 0;
        externalControl.pitch = 0;
        externalControl.roll = 0;
        externalControl.yaw = 0;
        externalControl.throttle = 0;

        // From main.c. What does it do??
        externalControl.digital[0] = 0x55;
}

int16_t* EC_getPRTY(void) {
        return EC_PRTY;
}

uint8_t EC_getArgument(void) {
        return externalControl.config;
}

uint8_t EC_getCommand(void) {
        return externalControl.free;
}

// not implemented.
int16_t EC_getVariable(uint8_t varNum) {
        return 0;
}

void EC_update() {
        if (externalControlActive) {
                externalControlActive--;
                EC_PRTY[CONTROL_PITCH] = (int16_t) externalControl.pitch
                                * (int16_t) staticParams.StickP;
                EC_PRTY[CONTROL_ROLL] = externalControl.roll
                                * (int16_t) staticParams.StickP;
                EC_PRTY[CONTROL_THROTTLE] = externalControl.throttle;
                EC_PRTY[CONTROL_YAW] = externalControl.yaw; // No stickP or similar??????
        } else {
                EC_PRTY[CONTROL_PITCH] = EC_PRTY[CONTROL_ROLL] = EC_PRTY[CONTROL_THROTTLE]
                                = EC_PRTY[CONTROL_YAW] = 0;
        }
}

uint8_t EC_getSignalQuality(void) {
        if (externalControlActive > 80)
                // Configured and heard from recently
                return SIGNAL_GOOD;

        if (externalControlActive)
                // Configured and heard from
                return SIGNAL_OK;

        if (!(externalControl.config & 0x01 && dynamicParams.ExternalControl > 128))
                // External control is not even configured.
                return NO_SIGNAL;

        // Configured but expired.
        return SIGNAL_LOST;
}