Did you noticed that lately I write about radios quite often? Well, I do and it's not a coincidence. Proper introduction for what I'm working on will happen in a next few days, but now I will only write that this will be a mid-range, cheap, DIY radio link for UAVs. By mid-range I mean up to 5km. So, it will be positioned somewhere between 2.4GHz systems and full sale LRSes like DragonLink or TBS Crossfire.

Back to business. I've discovered, that there is very little in The Internet how to generate S.Bus with Arduino. OK, there are few libraries for reading Futaba S.Bus protocol like mikeshub/FUTABA_SBUS or zendes/SBUS but the only library made simple I've found is bolderflight/SBUS. Too bad it works only with Teensy devices. So, after a few hours of hard work, reading code of OpenTX, MultiWii, INAV, reading RcGroups and final help of Konstantin Sharlaimov (Digital Entity of INAV), I give you:

Generate S.Bus packets with Arduino in a simple way

But first, few simple facts:

  • S.Bus protocol is INVERTED on a hardware level. That mean, Arduino can not talk directly with other S.Bus devices without additional inverter. Like this one
  • When talking to most modern flight controllers, inverter might not be required and long as flight controller can disable onboard inverters. F1 and F4 do not have build in inverters at all, so any UART can be used directly and in case of F3 and F7, INAV/Betaflight can disable inversion in software
  • Futaba S.Bus in "FASSTest 18CH" protocols encodes 16 RC channels and 2 digital channels (ON/OFF)
  • S.Bus encodes each RC channel with 11 bits
  • Internally, channel values are mapped as:
    • -100% = 173 (equivalent of 1000 in PWM servo signal)
    • 0% = 992 (equivalent of 1500 in PWM servo signal)
    • 100% = 1811 (equivalent of 2000 in PMW servo signal)
  • Serial port has to be configured as 100000bps, SERIAL_8E2 (8 bits, even, 2 stop bits)
  • Failsafe state is transfered via single is flags byte
  • One S.Bus packet takes 25 bytes
  • One frame is transmitted every 15ms (FASSTest 18CH mode)
#define RC_CHANNEL_MIN 990
#define RC_CHANNEL_MAX 2010

#define SBUS_MIN_OFFSET 173
#define SBUS_MID_OFFSET 992
#define SBUS_MAX_OFFSET 1811
#define SBUS_CHANNEL_NUMBER 16
#define SBUS_PACKET_LENGTH 25
#define SBUS_FRAME_HEADER 0x0f
#define SBUS_FRAME_FOOTER 0x00
#define SBUS_FRAME_FOOTER_V2 0x04
#define SBUS_STATE_FAILSAFE 0x08
#define SBUS_STATE_SIGNALLOSS 0x04
#define SBUS_UPDATE_RATE 15 //ms

void sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool isFailsafe){

    static int output[SBUS_CHANNEL_NUMBER] = {0};

    /*
     * Map 1000-2000 with middle at 1500 chanel values to
     * 173-1811 with middle at 992 S.BUS protocol requires
     */
    for (uint8_t i = 0; i < SBUS_CHANNEL_NUMBER; i++) {
        output[i] = map(channels[i], RC_CHANNEL_MIN, RC_CHANNEL_MAX, SBUS_MIN_OFFSET, SBUS_MAX_OFFSET);
    }

    uint8_t stateByte = 0x00;
    if (isSignalLoss) {
        stateByte |= SBUS_STATE_SIGNALLOSS;
    }
    if (isFailsafe) {
        stateByte |= SBUS_STATE_FAILSAFE;
    }
    packet[0] = SBUS_FRAME_HEADER; //Header

    packet[1] = (uint8_t) (output[0] & 0x07FF);
    packet[2] = (uint8_t) ((output[0] & 0x07FF)>>8 | (output[1] & 0x07FF)<<3);
    packet[3] = (uint8_t) ((output[1] & 0x07FF)>>5 | (output[2] & 0x07FF)<<6);
    packet[4] = (uint8_t) ((output[2] & 0x07FF)>>2);
    packet[5] = (uint8_t) ((output[2] & 0x07FF)>>10 | (output[3] & 0x07FF)<<1);
    packet[6] = (uint8_t) ((output[3] & 0x07FF)>>7 | (output[4] & 0x07FF)<<4);
    packet[7] = (uint8_t) ((output[4] & 0x07FF)>>4 | (output[5] & 0x07FF)<<7);
    packet[8] = (uint8_t) ((output[5] & 0x07FF)>>1);
    packet[9] = (uint8_t) ((output[5] & 0x07FF)>>9 | (output[6] & 0x07FF)<<2);
    packet[10] = (uint8_t) ((output[6] & 0x07FF)>>6 | (output[7] & 0x07FF)<<5);
    packet[11] = (uint8_t) ((output[7] & 0x07FF)>>3);
    packet[12] = (uint8_t) ((output[8] & 0x07FF));
    packet[13] = (uint8_t) ((output[8] & 0x07FF)>>8 | (output[9] & 0x07FF)<<3);
    packet[14] = (uint8_t) ((output[9] & 0x07FF)>>5 | (output[10] & 0x07FF)<<6);  
    packet[15] = (uint8_t) ((output[10] & 0x07FF)>>2);
    packet[16] = (uint8_t) ((output[10] & 0x07FF)>>10 | (output[11] & 0x07FF)<<1);
    packet[17] = (uint8_t) ((output[11] & 0x07FF)>>7 | (output[12] & 0x07FF)<<4);
    packet[18] = (uint8_t) ((output[12] & 0x07FF)>>4 | (output[13] & 0x07FF)<<7);
    packet[19] = (uint8_t) ((output[13] & 0x07FF)>>1);
    packet[20] = (uint8_t) ((output[13] & 0x07FF)>>9 | (output[14] & 0x07FF)<<2);
    packet[21] = (uint8_t) ((output[14] & 0x07FF)>>6 | (output[15] & 0x07FF)<<5);
    packet[22] = (uint8_t) ((output[15] & 0x07FF)>>3);

    packet[23] = stateByte; //Flags byte
    packet[24] = SBUS_FRAME_FOOTER; //Footer
}

uint8_t sbusPacket[SBUS_PACKET_LENGTH];
int rcChannels[SBUS_CHANNEL_NUMBER];
uint32_t sbusTime = 0;

void setup() {
    for (uint8_t i = 0; i < SBUS_CHANNEL_NUMBER; i++) {
        rcChannels[i] = 1500;
    }
    Serial.begin(100000, SERIAL_8E2);
}

void loop() {
    uint32_t currentMillis = millis();

    /*
     * Here you can modify values of rcChannels while keeping it in 1000:2000 range
     */

    if (currentMillis > sbusTime) {
        sbusPreparePacket(sbusPacket, rcChannels, false, false);
        Serial.write(sbusPacket, SBUS_PACKET_LENGTH);

        sbusTime = currentMillis + SBUS_UPDATE_RATE;
    }
}

I hope code above is simple enough. setup method initializes serial port in correct mode Serial.begin(100000, SERIAL_8E2); and loop encodes and send S.Bus frame every 15ms. Packet encoding can be done in a better way, but hey, at least it is working!