File content as of revision 15:c96df23cad7d:

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

SBUS::SBUS(PinName tx, PinName rx) :
sbus(tx, rx),

//CALIBRATE INPUT SIGNALS (USE getChannelValue)

sbusMaximumValue(1811),
sbusNeutralValue(992),
sbusMinimumValue(172),
sbusDeadband(10)

{
    sbus.baud(100000);
    sbus.attach(callback(this, &SBUS::receiveData), Serial::RxIrq);      //Z TEMLE BEREMO UART ZMER KO PRIDEJO PODATKI
    sbus.format(8, Serial::Even, 2);
}

int SBUS::checkFailsafeTimer() {
    failsafetime = 12;
    if(lastreadtime < failsafetime){
        reportreadtime = lastreadtime;
        lastreadtime = failsafetime + 1;
        return 1;
    }
    else{
        return 0;
    }
}

int SBUS::failSafeTimerMs() {
    return reportreadtime;
}

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

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

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

void SBUS::decordReceivedData() {
    failsafetimer.stop();
    lastreadtime = failsafetimer.read_ms();
    failsafetimer.reset();
    failsafetimer.start();
    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 < 16; i++) {
        float buf;
        if(channel[i] > (sbusNeutralValue + sbusDeadband)){                                                           //THIS IS MIDDLE DEADBAND (DEAD ZONE)
            buf = ((float)(channel[i] - sbusNeutralValue) / (float)(sbusMaximumValue - sbusNeutralValue)); 
        }   
        else if(channel[i] < (sbusNeutralValue - sbusDeadband)){                                                      //THIS IS MIDDLE DEADBAND (DEAD ZONE)
            buf = -((float)(channel[i] - sbusNeutralValue) / (float)(sbusMinimumValue - sbusNeutralValue));
        }
        else buf = 0.0f;
        buf = (int)(buf*100)/100.0f;             //Limit output resolution to 2 decimals (x.yz)           
        if(buf > 1.0f){                  //Limit to -1.0 to 1.0
            buf = 1.0f;
        }
        if(buf < -1.0f){
            buf = -1.0f;
        }
        stickValue[i] = buf;
    }
}