41 lines
948 B
C
41 lines
948 B
C
/**
|
||
* @file sbus.c
|
||
* @brief SBUS decoder implementation
|
||
*/
|
||
#include <string.h>
|
||
#include "sbus.h"
|
||
|
||
void sbus_init(void)
|
||
{
|
||
/* Nothing to initialize */
|
||
}
|
||
|
||
void sbus_decode(const sbus_frame_t *frame, uint16_t *channels, uint8_t *flags)
|
||
{
|
||
if (frame->start_byte != 0x0F) {
|
||
/* Invalid frame */
|
||
if (flags) *flags = 0;
|
||
return;
|
||
}
|
||
|
||
/* Unpack 16 × 11-bit channels from 22 bytes, little-endian */
|
||
const uint8_t *data = frame->channel_data;
|
||
for (int i = 0; i < 16; i++) {
|
||
int byte_idx = (i * 11) / 8;
|
||
int bit_off = (i * 11) % 8;
|
||
|
||
uint16_t ch = data[byte_idx] >> bit_off;
|
||
ch |= (uint16_t)(data[byte_idx + 1]) << (8 - bit_off);
|
||
if (bit_off > 5) {
|
||
ch |= (uint16_t)(data[byte_idx + 2]) << (16 - bit_off);
|
||
}
|
||
|
||
channels[i] = ch & 0x07FF;
|
||
}
|
||
|
||
/* Extract flags */
|
||
if (flags) {
|
||
*flags = frame->flags;
|
||
}
|
||
}
|