celrs/celrs/crsf.c

#include "celrs/crsf.h"
#include <string.h>

/* --------------------------------------------------------------------------- */
/* CRC8/DVB-S2 lookup table (poly 0xD5)                                       */
/* --------------------------------------------------------------------------- */

static uint8_t s_crc8_table[256];
static int s_crc8_init = 0;

static void crc8_build_table(void) {
    for (int i = 0; i < 256; i++) {
        uint8_t crc = (uint8_t)i;
        for (int j = 0; j < 8; j++) {
            crc = (crc & 0x80)
                ? ((crc << 1) ^ 0xD5)
                :  (crc << 1);
        }
        s_crc8_table[i] = crc;
    }
    s_crc8_init = 1;
}

uint8_t cel_crsf_crc(uint8_t const* data, size_t len) {
    if (!s_crc8_init) crc8_build_table();
    uint8_t crc = 0x00;
    for (size_t i = 0; i < len; i++) {
        crc = s_crc8_table[crc ^ data[i]];
    }
    return crc;
}

/* --------------------------------------------------------------------------- */
/* Frame builders                                                             */
/* --------------------------------------------------------------------------- */

size_t cel_crsf_build_rc_frame(uint8_t* dst, int16_t const channels[16]) {
    if (dst == NULL) return 0;

    /* Pack 16 channels (11-bit each) into 22 bytes */
    int16_t ch[16];
    if (channels != NULL) {
        memcpy(ch, channels, sizeof(ch));
    } else {
        memset(ch, 0, sizeof(ch));
    }
    for (int i = 0; i < 16; i++) {
        ch[i] = cel_crsf_channel_clamp(ch[i]);
    }

    /* Pack into 22 bytes */
    uint8_t packed[22];
    for (int i = 0; i < 16; i++) {
        int16_t val = ch[i] - CEL_CRSF_CH_MIN; /* 0..1023 */
        int idx = i / 2;
        if (i % 2 == 0) {
            packed[idx * 2] = (uint8_t)(val & 0x7FF);
            packed[idx * 2 + 1] = (uint8_t)((val >> 8) | ((val >> 0) & 0x18) << 2);
        }
    }

    /* Build frame: [addr][length][type][payload...][crc] */
    uint8_t length = 1 + 22 + 1; /* type + payload + crc */
    dst[0] = 0xC8; /* RC frame address */
    dst[1] = length;
    dst[2] = CEL_CRSF_TYPE_RC_CHANNELS_PACKED;
    memcpy(dst + 3, packed, 22);
    uint8_t crc = cel_crsf_crc(dst + 2, 1 + 22);
    dst[2 + length - 1] = crc;
    return 2 + length;
}

size_t cel_crsf_build_ping_frame(uint8_t* dst) {
    if (dst == NULL) return 0;

    uint8_t length = 1 + 2 + 1; /* type + payload(2) + crc */
    dst[0] = CEL_CRSF_ADDRESS_MODULE;
    dst[1] = length;
    dst[2] = CEL_CRSF_TYPE_DEVICE_PING;
    dst[3] = CEL_CRSF_ADDRESS_FC_BROADCAST; /* dest */
    dst[4] = CEL_CRSF_ADDRESS_CUSTOM_MODULE; /* src */
    uint8_t crc = cel_crsf_crc(dst + 2, 1 + 2);
    dst[2 + length - 1] = crc;
    return 2 + length;
}

size_t cel_crsf_build_param_read_frame(uint8_t* dst, uint8_t index,
                                        uint8_t chunk) {
    if (dst == NULL) return 0;

    uint8_t length = 1 + 4 + 1; /* type + payload(4) + crc */
    dst[0] = CEL_CRSF_ADDRESS_MODULE;
    dst[1] = length;
    dst[2] = CEL_CRSF_TYPE_PARAM_READ;
    dst[3] = CEL_CRSF_ADDRESS_MODULE;       /* dest */
    dst[4] = CEL_CRSF_ADDRESS_LUA;          /* src */
    dst[5] = index;
    dst[6] = chunk;
    uint8_t crc = cel_crsf_crc(dst + 2, 1 + 4);
    dst[2 + length - 1] = crc;
    return 2 + length;
}

size_t cel_crsf_build_param_write_frame(uint8_t* dst, uint8_t index,
                                         uint8_t value) {
    if (dst == NULL) return 0;

    uint8_t length = 1 + 4 + 1; /* type + payload(4) + crc */
    dst[0] = CEL_CRSF_ADDRESS_MODULE;
    dst[1] = length;
    dst[2] = CEL_CRSF_TYPE_PARAM_WRITE;
    dst[3] = CEL_CRSF_ADDRESS_MODULE;       /* dest */
    dst[4] = CEL_CRSF_ADDRESS_LUA;          /* src */
    dst[5] = index;
    dst[6] = value;
    uint8_t crc = cel_crsf_crc(dst + 2, 1 + 4);
    dst[2 + length - 1] = crc;
    return 2 + length;
}

/* --------------------------------------------------------------------------- */
/* Frame parser                                                               */
/* --------------------------------------------------------------------------- */

int cel_crsf_frame_parse(cel_crsf_frame* frame, uint8_t const* buf,
                          size_t len) {
    if (frame == NULL || buf == NULL) return -1;
    /* Minimum: addr(1) + length(1) + type(1) + crc(1) = 4 bytes */
    if (len < 4) return -1;

    uint8_t addr = buf[0];
    uint8_t length = buf[1];
    size_t total = 2 + length;
    if (len < total) return -1;

    uint8_t type = buf[2];
    size_t payload_len = length > 1 ? length - 2 : 0;
    uint8_t crc_recv = buf[total - 1];

    /* CRC over type + payload */
    uint8_t crc_calc = cel_crsf_crc(buf + 2, 1 + payload_len);
    if (crc_calc != crc_recv) return -1;

    frame->addr = addr;
    frame->length = length;
    frame->type = type;
    frame->payload_len = (uint8_t)payload_len;
    if (payload_len > 0) {
        memcpy(frame->payload, buf + 3, payload_len);
    }
    frame->crc = crc_recv;
    return 0;
}

/* --------------------------------------------------------------------------- */
/* Channel helpers                                                            */
/* --------------------------------------------------------------------------- */

int16_t cel_crsf_channel_clamp(int16_t value) {
    if (value < CEL_CRSF_CH_MIN) return CEL_CRSF_CH_MIN;
    if (value > CEL_CRSF_CH_MAX) return CEL_CRSF_CH_MAX;
    return value;
}

int16_t cel_crsf_channel_us_to_val(uint16_t us) {
    /* Clamp to valid range first to avoid underflow */
    if (us < 988) us = 988;
    if (us > 2012) us = 2012;
    /* Round to nearest: add half the divisor before dividing */
    int16_t value = (int16_t)(
        ((us - 988) * (CEL_CRSF_CH_MAX - CEL_CRSF_CH_MIN)
         + (2012 - 988) / 2) / (2012 - 988)
        + CEL_CRSF_CH_MIN);
    return cel_crsf_channel_clamp(value);
}

uint16_t cel_crsf_channel_val_to_us(int16_t value) {
    value = cel_crsf_channel_clamp(value);
    return (uint16_t)(
        ((value - CEL_CRSF_CH_MIN) * (2012 - 988)
         + (CEL_CRSF_CH_MAX - CEL_CRSF_CH_MIN) / 2)
        / (CEL_CRSF_CH_MAX - CEL_CRSF_CH_MIN) + 988);
}

void cel_crsf_channel_default(int16_t channels[16]) {
    memset(channels, 0, sizeof(int16_t) * 16);
    channels[0] = CEL_CRSF_CH_MID; /* roll */
    channels[1] = CEL_CRSF_CH_MID; /* pitch */
    channels[2] = CEL_CRSF_CH_MIN; /* throttle */
    channels[3] = CEL_CRSF_CH_MID; /* yaw */
    for (int i = 4; i < 16; i++) {
        channels[i] = CEL_CRSF_CH_MIN;
    }
}

/* --------------------------------------------------------------------------- */
/* Telemetry parsing                                                          */
/* --------------------------------------------------------------------------- */

/* See crsf_telemetry.c */