File content as of revision 1:28f9c5332835:

#include "sbus.h"

Sbus::Sbus(PinName tx,PinName rx) : com(tx,rx) {
    com.baud(100000);
    com.attach(this, &Sbus::serialReceiveHandler, Serial::RxIrq);
    com.format(8, Serial::Even, 2);
    
    stickMaximumValue = 0x0690;
    stickNeutralValue = 0x0400;
    stickMinimumValue = 0x0170;
    stickResolution   = stickMaximumValue - stickMinimumValue;
    
    enableReceive = false;
    failsafe = SBUS_SIGNAL_FAILSAFE;
    
    resetAllData();
    receiveTimer.start();
}

void Sbus::setup(int ch5,int ch6,int ch7,int ch8,int ch9,int ch10) {
    swSelectData[0] = ch5;
    swSelectData[1] = ch6;
    swSelectData[2] = ch7;
    swSelectData[3] = ch8;
    swSelectData[4] = ch9;
    swSelectData[5] = ch10;
}

void Sbus::serialReceiveHandler() {
    static int data_count = 0;
    
    char buf;
    buf = com.getc();
    
    if(receiveTimer.read() > 0.006f)  {
        if(buf == 0x0F) {
            data_count = 0;
            enableReceive = true;
            receiveTimer.reset();
        } else{
            enableReceive = false; 
        }      
    }
    
    if(enableReceive)    receiveTimer.reset();
    
    if(enableReceive) {
        if(data_count < 25) {
            receiveTimer.reset();
            receiveData[data_count] = static_cast<int>(buf);
            data_count++;
        }
    }
    
    if(data_count == 25) {
        if(receiveData[0] == 0x0F) {
            decordReceiveData();
        } else {
            resetAllData();
        }
        data_count = 0;
        enableReceive = false;
    }
}

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

void Sbus::convertReceiveData() {
    for(int i=ANALOG_RX;i<=ANALOG_LY;i++) {
        float buf = 0.0f;
        if(channel[i] > (stickNeutralValue + 20)) buf = ((channel[i] - stickNeutralValue) / (float)(stickMaximumValue - stickNeutralValue));
        else if(channel[i] < (stickNeutralValue - 20)) buf = -((channel[i] - stickNeutralValue) / (float)(stickMinimumValue - stickNeutralValue));
        else buf = 0.0f;
        
        buf = static_cast<float>((buf*100)/100.0f);
        if(buf > 1.0f)          buf = 1.0f;
        else if(buf < -1.0f)    buf = -1.0f;
        
        switch (i) {
            case 0:
                stickValue[ANALOG_RX] = buf;
                break;
            case 1:
                stickValue[ANALOG_LY] = -buf;
                break;
            case 2:
                stickValue[ANALOG_RY] = buf;
                break;
            case 3:
                stickValue[ANALOG_LX] = buf;
                break;
        }
    }
    
    
    for(int i = 0; i < 6; i++) {
        if(swSelectData[i] == VR) {
            float vr_buf = 0.0f;
            if(channel[4] > (stickNeutralValue + 20)) vr_buf = ((channel[4] - stickNeutralValue) / (float)(stickMaximumValue - stickNeutralValue));
            else if(channel[4] < (stickNeutralValue - 20)) vr_buf = -((channel[4] - stickNeutralValue) / (float)(stickMinimumValue - stickNeutralValue));
            else vr_buf = 0.0f;
            
            vr_buf = (int)(vr_buf*100)/100.0f;
            if(vr_buf > 1.0f) vr_buf = 1.0f;
            else if(vr_buf < -1.0f) vr_buf = -1.0f;
            volumeValue = vr_buf;
        }
    }        

    for(int i = 0; i < 6; i++) {
        switch(channel[i + 4]) {
            case 0x078b:
            case 0x770:
                switchValue[swSelectData[i]] = HIGH;
                break;
            case 0x0400:
                switchValue[swSelectData[i]] = NEUTRAL;
                break;
            case 0x0090:
                switchValue[swSelectData[i]] = LOW;
                break;
            default:
                switchValue[i] = NEUTRAL;
                break;
        }
    }
    
    failsafe = SBUS_SIGNAL_OK;
    if (receiveData[23] & (1<<2)) {
        failsafe = SBUS_SIGNAL_LOST;
    }
    if (receiveData[23] & (1<<3)) {
        failsafe = SBUS_SIGNAL_FAILSAFE;
    }
}

void Sbus::resetAllData() {
    for(int i = 0; i < 16; i++) channel[i] = 0;
    for(int i = 0; i < 50; i++) receiveData[i] = 0;
    for(int i = 0; i < 4; i++) stickValue[i] = 0.0f;
    for(int i = 0; i < 12; i++) switchValue[i] = NEUTRAL;
    volumeValue = 0.0f;
}

float Sbus::getStickValue(int ch) {
    return stickValue[ch];
}

float Sbus::getVolumeValue() {
    return volumeValue;   
}

int Sbus::getSwitchValue(int ch) {
    return switchValue[ch];
}

int Sbus::getChannelValue(int ch) {
    return channel[ch];
}

int Sbus::getFailSafe() {
    return failsafe;   
}