File content as of revision 2:493d10424466:

#include "mbed.h"
#include "SBUS.h"

SBUS::SBUS(PinName tx, PinName rx) :
com(tx, rx),
stickMaximumValue(0x068F),
stickNeutralValue(0x03FF),
stickMinimumValue(0x016F),
stickResolution(stickMaximumValue - stickMinimumValue)
{
    com.baud(100000);
    com.attach(this, &SBUS::receiveData, Serial::RxIrq);
    com.format(8, Serial::Even, 2);
    switchPositionValue[0] = 0x076F;
    switchPositionValue[1] = 0x03FF;
    switchPositionValue[2] = 0x008F;
    switchFPositionValue[0] = 0x076F;
    switchFPositionValue[1] = 0x0074;
}

float SBUS::getStickValue(int tag) {
    return stickValue[tag];
}

int SBUS::getSwitchValue(int tag) {
    return switchValue[tag];
}

int SBUS::getChannelValue(int tag) {
    return channel[tag];
}

void SBUS::receiveData() {
    static int count = 0;
    char buf;
    
    buf = ~com.getc();
    
    if(buf == 0xF0) {
        decordReceivedData();
        count = 0;
    }
    if(count >= 25) return;
    receivedData[count] = buf;
    count++;
}

void SBUS::decordReceivedData() {
    channel[0]  = ((receivedData[1]    |receivedData[2]<<8)                 & 0x07FF);
    channel[1]  = ((receivedData[2]>>3 |receivedData[3]<<5)                 & 0x07FF);
    channel[2]  = ((receivedData[3]>>6 |receivedData[4]<<2 |receivedData[5]<<10)  & 0x07FF);
    channel[3]  = ((receivedData[5]>>1 |receivedData[6]<<7)                 & 0x07FF);
    channel[4]  = ((receivedData[6]>>4 |receivedData[7]<<4)                 & 0x07FF);
    channel[5]  = ((receivedData[7]>>7 |receivedData[8]<<1 |receivedData[9]<<9)   & 0x07FF);
    channel[6]  = ((receivedData[9]>>2 |receivedData[10]<<6)                & 0x07FF);
    channel[7]  = ((receivedData[10]>>5|receivedData[11]<<3)                & 0x07FF);
    channel[8]  = ((receivedData[12]   |receivedData[13]<<8)                & 0x07FF);
    channel[9]  = ((receivedData[13]>>3|receivedData[14]<<5)                & 0x07FF);
    channel[10] = ((receivedData[14]>>6|receivedData[15]<<2|receivedData[16]<<10) & 0x07FF);
    channel[11] = ((receivedData[16]>>1|receivedData[17]<<7)                & 0x07FF);
    channel[12] = ((receivedData[17]>>4|receivedData[18]<<4)                & 0x07FF);
    channel[13] = ((receivedData[18]>>7|receivedData[19]<<1|receivedData[20]<<9)  & 0x07FF);
    channel[14] = ((receivedData[20]>>2|receivedData[21]<<6)                & 0x07FF);
    channel[15] = ((receivedData[21]>>5|receivedData[22]<<3)                & 0x07FF);
    convertReceivedData();
}

void SBUS::convertReceivedData() {
    for(int i = 0; i < 4; i++) {
        float buf;
        if(channel[i] > stickNeutralValue) buf = ((channel[i] - stickNeutralValue) / (float)(stickMaximumValue - stickNeutralValue));
        else buf = -((channel[i] - stickNeutralValue) / (float)(stickMinimumValue - stickNeutralValue));
        switch(i) {
            case right_RL:
                stickValue[analog_rx] = -buf;
                break;
            case right_UD:
                stickValue[analog_ry] = -buf;
                break;
            case left_RL:
                stickValue[analog_lx] = -buf;
                break;
            case left_UD:
                stickValue[analog_ly] = buf;
                break;
            default:
                break;
        }
    }
    for(int i = 0; i < 12; i++) {
        switch(channel[i + 4]) {
            case 0x008F:
            case 0x0074:
                switchValue[i] = High;
                break;
            case 0x03FF:
                switchValue[i] = Neutral;
                break;
            case 0x076F:
                switchValue[i] = Low;
                break;
            default:
                switchValue[i] = channel[i + 3] / 1903.0f;
                break;
        }
    }
}