Function decode

SBUS/SBUS.cpp:23–48

function to decode the received SBUS data to actual steering commands

// function to decode the received SBUS data to actual steering commands
void decode(uint8_t *data)
{
    uint16_t channels[16];
    // see https://platformio.org/lib/show/5622/Bolder%20Flight%20Systems%20SBUS
    channels[0] = (uint16_t)((data[0] | data[1] << 8) & 0x07FF);
    channels[1] = (uint16_t)((data[1] >> 3 | data[2] << 5) & 0x07FF);
    channels[2] = (uint16_t)((data[2] >> 6 | data[3] << 2 | data[4] << 10) & 0x07FF);
    channels[3] = (uint16_t)((data[4] >> 1 | data[5] << 7) & 0x07FF);
    channels[4] = (uint16_t)((data[5] >> 4 | data[6] << 4) & 0x07FF);
    channels[5] = (uint16_t)((data[6] >> 7 | data[7] << 1 | data[8] << 9) & 0x07FF);
    channels[6] = (uint16_t)((data[8] >> 2 | data[9] << 6) & 0x07FF);
    channels[7] = (uint16_t)((data[9] >> 5 | data[10] << 3) & 0x07FF);
    channels[8] = (uint16_t)((data[11] | data[12] << 8) & 0x07FF);
    channels[9] = (uint16_t)((data[12] >> 3 | data[13] << 5) & 0x07FF);
    channels[10] = (uint16_t)((data[13] >> 6 | data[14] << 2 | data[15] << 10) & 0x07FF);
    channels[11] = (uint16_t)((data[15] >> 1 | data[16] << 7) & 0x07FF);
    channels[12] = (uint16_t)((data[16] >> 4 | data[17] << 4) & 0x07FF);
    channels[13] = (uint16_t)((data[17] >> 7 | data[18] << 1 | data[19] << 9) & 0x07FF);
    channels[14] = (uint16_t)((data[19] >> 2 | data[20] << 6) & 0x07FF);
    channels[15] = (uint16_t)((data[20] >> 5 | data[21] << 3) & 0x07FF);
    for (int i = 0; i < 16; i++)
    {
        printf("%d \t", channels[i]);
    }
    printf("\n");
}

// Main function
int main()