portersky/celrs
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: implement frame parse and streaming reader

Browse Source 
cel_crsf_frame_parse() parses ELRS USB format frames:
  [addr][length][type][payload...][crc]

cel_crsf_stream_* provides incremental parsing from a byte
stream: skips invalid sync bytes, discards bad CRC frames,
buffers partial frames across feed calls.
This commit is contained in:
portersky
2026-06-14 20:51:57 +02:00
parent dde27ab566
commit a846b063f9
5 changed files with 335 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
celrs/crsf.c
+26 -10
View File
@@ -85,16 +85,32 @@ size_t cel_crsf_build_param_write_frame(uint8_t* dst, uint8_t index,
int cel_crsf_frame_parse(cel_crsf_frame* frame, uint8_t const* buf,
size_t len) {
/* TODO: parse ELRS-format frame [addr][length][type][payload...][crc].
* Validate:
* - buf has at least 4 bytes (addr + length + type + crc minimum)
* - total frame = 2 (addr+length) + length bytes
* - CRC over buf[2..2+length-1] matches buf[2+length-1]
* Copy type and payload into frame struct. */
(void)frame;
(void)buf;
(void)len;
return -1;
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;
}
/* --------------------------------------------------------------------------- */
celrs/crsf_stream.c
+48 -25
View File
@@ -15,44 +15,67 @@ static int is_valid_addr(uint8_t addr) {
}
struct cel_crsf_stream {
uint8_t buf[260]; /* internal buffer (max frame = 2 + 255 + 1 = 258) */
uint8_t buf[260]; /* max frame = 2 + 255 + 1 = 258 */
size_t buf_len;
};
cel_crsf_stream* cel_crsf_stream_create(void) {
/* TODO: allocate and zero-initialize the stream struct. */
return NULL;
cel_crsf_stream* s = calloc(1, sizeof(*s));
return s;
}
void cel_crsf_stream_destroy(cel_crsf_stream* stream) {
/* TODO: free the stream struct. */
(void)stream;
free(stream);
}
int cel_crsf_stream_feed(cel_crsf_stream* stream, uint8_t const* data,
size_t len, cel_crsf_frame* out,
size_t out_capacity) {
/* TODO: append data to internal buffer, scan for complete frames.
* Algorithm:
* 1. Copy incoming bytes into internal buffer.
* 2. While buffer has >= 4 bytes:
* a. If buf[0] is not a valid address byte, pop(0) and continue.
* b. Read length = buf[1].
* c. Total frame = 2 + length.
* d. If buffer < total, break (need more data).
* e. Parse frame from buf[0..total-1].
* f. On success (CRC valid), copy into out[] and advance buffer.
* g. On failure (CRC invalid), discard frame and continue.
* 3. Return number of valid frames parsed, or -1 on error. */
(void)stream;
(void)data;
(void)len;
(void)out;
(void)out_capacity;
return 0;
if (stream == NULL) return -1;
if (out == NULL) return -1;
if (data == NULL && len > 0) return -1;
if (data == NULL && len == 0) {
/* Drain remaining frames from buffer without new data */
len = 0;
}
/* Append new data to buffer */
if (len > 0) {
if (stream->buf_len + len > sizeof(stream->buf)) {
/* Buffer overflow — discard everything */
stream->buf_len = 0;
return -1;
}
memcpy(stream->buf + stream->buf_len, data, len);
stream->buf_len += len;
}
int count = 0;
while (stream->buf_len >= 4 && count < (int)out_capacity) {
/* Skip invalid sync bytes */
if (!is_valid_addr(stream->buf[0])) {
memmove(stream->buf, stream->buf + 1, stream->buf_len - 1);
stream->buf_len--;
continue;
}
uint8_t length = stream->buf[1];
size_t total = 2 + length;
if (stream->buf_len < total) break; /* need more data */
/* Try to parse frame */
cel_crsf_frame frame;
if (cel_crsf_frame_parse(&frame, stream->buf, total) == 0) {
out[count++] = frame;
}
/* Discard frame (valid or not) */
memmove(stream->buf, stream->buf + total, stream->buf_len - total);
stream->buf_len -= total;
}
return count;
}
void cel_crsf_stream_reset(cel_crsf_stream* stream) {
/* TODO: zero buf_len to discard any partial frame. */
(void)stream;
stream->buf_len = 0;
}
tests/CMakeLists.txt
+8
View File
@@ -38,6 +38,14 @@ target_compile_features(test_crsf PRIVATE c_std_23)
add_test(NAME test_crsf COMMAND test_crsf)
list(APPEND TEST_TARGETS test_crsf)
# CRSF stream tests
add_executable(test_crsf_stream test_crsf_stream.c)
target_include_directories(test_crsf_stream PRIVATE "${CMAKE_SOURCE_DIR}")
target_link_libraries(test_crsf_stream PRIVATE celrs_crsf Unity::Unity)
target_compile_features(test_crsf_stream PRIVATE c_std_23)
add_test(NAME test_crsf_stream COMMAND test_crsf_stream)
list(APPEND TEST_TARGETS test_crsf_stream)
# Serial tests — mocks the platform backend (serial_internal.h)
add_executable(test_serial test_serial.c)
target_include_directories(test_serial PRIVATE "${CMAKE_SOURCE_DIR}")
tests/test_crsf.c
+84 -1
View File
@@ -18,7 +18,6 @@ void test_crc_single_byte(void) {
}
void test_crc_known_value(void) {
/* Verify CRC is deterministic */
uint8_t data[6] = {0x10, 0x80, 0x03, 0x02, 0x80, 0x01};
uint8_t crc = cel_crsf_crc(data, 6);
TEST_ASSERT_TRUE(crc != 0);
@@ -39,6 +38,83 @@ void test_channel_clamp_mid(void) {
TEST_ASSERT_EQUAL_INT16(CEL_CRSF_CH_MID, cel_crsf_channel_clamp(CEL_CRSF_CH_MID));
}
/* Frame parse tests — ELRS format: [addr][length][type][payload][crc] */
/* Build a valid test frame with known CRC */
static void build_test_frame(uint8_t* dst, uint8_t addr, uint8_t type,
uint8_t const* payload, uint8_t payload_len) {
uint8_t length = 1 + payload_len + 1; /* type + payload + crc */
dst[0] = addr;
dst[1] = length;
dst[2] = type;
memcpy(dst + 3, payload, payload_len);
uint8_t crc = cel_crsf_crc(dst + 2, 1 + payload_len);
dst[2 + length - 1] = crc;
}
void test_parse_valid_frame(void) {
uint8_t buf[32];
uint8_t payload[2] = {0x80, 0x01};
build_test_frame(buf, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, payload, 2);
cel_crsf_frame frame;
TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, buf, sizeof(buf)));
TEST_ASSERT_EQUAL_UINT8(0xC8, frame.addr);
TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_HEARTBEAT, frame.type);
TEST_ASSERT_EQUAL_UINT8(2, frame.payload_len);
TEST_ASSERT_EQUAL_UINT8(0x80, frame.payload[0]);
TEST_ASSERT_EQUAL_UINT8(0x01, frame.payload[1]);
}
void test_parse_null_frame(void) {
uint8_t buf[8];
TEST_ASSERT_EQUAL_INT(-1, cel_crsf_frame_parse(NULL, buf, 8));
}
void test_parse_null_buf(void) {
cel_crsf_frame frame;
TEST_ASSERT_EQUAL_INT(-1, cel_crsf_frame_parse(&frame, NULL, 8));
}
void test_parse_too_short(void) {
cel_crsf_frame frame;
uint8_t buf[2] = {0xC8, 0x03};
TEST_ASSERT_EQUAL_INT(-1, cel_crsf_frame_parse(&frame, buf, 2));
}
void test_parse_bad_crc(void) {
uint8_t buf[32];
uint8_t payload[2] = {0x80, 0x01};
build_test_frame(buf, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, payload, 2);
buf[5] ^= 0xFF; /* corrupt the CRC */
cel_crsf_frame frame;
TEST_ASSERT_EQUAL_INT(-1, cel_crsf_frame_parse(&frame, buf, sizeof(buf)));
}
void test_parse_empty_payload(void) {
uint8_t buf[32];
build_test_frame(buf, 0xEE, CEL_CRSF_TYPE_HEARTBEAT, NULL, 0);
cel_crsf_frame frame;
TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, buf, sizeof(buf)));
TEST_ASSERT_EQUAL_UINT8(0xEE, frame.addr);
TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_HEARTBEAT, frame.type);
TEST_ASSERT_EQUAL_UINT8(0, frame.payload_len);
}
void test_parse_module_addr(void) {
uint8_t buf[32];
uint8_t payload[4] = {0xAA, 0xBB, 0xCC, 0xDD};
build_test_frame(buf, 0xEE, CEL_CRSF_TYPE_GPS, payload, 4);
cel_crsf_frame frame;
TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, buf, sizeof(buf)));
TEST_ASSERT_EQUAL_UINT8(0xEE, frame.addr);
TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_GPS, frame.type);
TEST_ASSERT_EQUAL_UINT8(4, frame.payload_len);
}
int main(void) {
UNITY_BEGIN();
RUN_TEST(test_crc_empty);
@@ -47,5 +123,12 @@ int main(void) {
RUN_TEST(test_channel_clamp_min);
RUN_TEST(test_channel_clamp_max);
RUN_TEST(test_channel_clamp_mid);
RUN_TEST(test_parse_valid_frame);
RUN_TEST(test_parse_null_frame);
RUN_TEST(test_parse_null_buf);
RUN_TEST(test_parse_too_short);
RUN_TEST(test_parse_bad_crc);
RUN_TEST(test_parse_empty_payload);
RUN_TEST(test_parse_module_addr);
return UNITY_END();
}
tests/test_crsf_stream.c
+169
View File
@@ -0,0 +1,169 @@
#include "unity.h"
#include "celrs/crsf.h"
#include <string.h>
void setUp(void) {}
void tearDown(void) {}
/* Helper: build a valid test frame, return total bytes */
static size_t build_frame(uint8_t* dst, uint8_t addr, uint8_t type,
uint8_t const* payload, uint8_t payload_len) {
uint8_t length = 1 + payload_len + 1; /* type + payload + crc */
dst[0] = addr;
dst[1] = length;
dst[2] = type;
memcpy(dst + 3, payload, payload_len);
uint8_t crc = cel_crsf_crc(dst + 2, 1 + payload_len);
dst[2 + length - 1] = crc;
return 2 + length;
}
/* Stream tests */
void test_stream_create_destroy(void) {
cel_crsf_stream* s = cel_crsf_stream_create();
TEST_ASSERT_NOT_NULL(s);
cel_crsf_stream_destroy(s);
}
void test_stream_feed_empty(void) {
cel_crsf_stream* s = cel_crsf_stream_create();
cel_crsf_frame frames[4];
int n = cel_crsf_stream_feed(s, NULL, 0, frames, 4);
TEST_ASSERT_EQUAL_INT(0, n);
cel_crsf_stream_destroy(s);
}
void test_stream_feed_single_frame(void) {
uint8_t buf[32];
uint8_t payload[2] = {0x80, 0x01};
size_t len = build_frame(buf, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, payload, 2);
cel_crsf_stream* s = cel_crsf_stream_create();
cel_crsf_frame frames[4];
int n = cel_crsf_stream_feed(s, buf, len, frames, 4);
TEST_ASSERT_EQUAL_INT(1, n);
TEST_ASSERT_EQUAL_UINT8(0xC8, frames[0].addr);
TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_HEARTBEAT, frames[0].type);
TEST_ASSERT_EQUAL_UINT8(2, frames[0].payload_len);
TEST_ASSERT_EQUAL_UINT8(0x80, frames[0].payload[0]);
cel_crsf_stream_destroy(s);
}
void test_stream_feed_incremental(void) {
uint8_t buf[32];
uint8_t payload[2] = {0x80, 0x01};
size_t total = build_frame(buf, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, payload, 2);
cel_crsf_stream* s = cel_crsf_stream_create();
cel_crsf_frame frames[4];
/* Feed first 3 bytes — not enough for a complete frame */
int n = cel_crsf_stream_feed(s, buf, 3, frames, 4);
TEST_ASSERT_EQUAL_INT(0, n);
/* Feed remaining bytes — now complete */
n = cel_crsf_stream_feed(s, buf + 3, total - 3, frames, 4);
TEST_ASSERT_EQUAL_INT(1, n);
TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_HEARTBEAT, frames[0].type);
cel_crsf_stream_destroy(s);
}
void test_stream_feed_multiple_frames(void) {
uint8_t buf[64];
uint8_t p1[2] = {0x80, 0x01};
uint8_t p2[3] = {0xAA, 0xBB, 0xCC};
size_t len1 = build_frame(buf, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, p1, 2);
size_t len2 = build_frame(buf + len1, 0xEE, CEL_CRSF_TYPE_GPS, p2, 3);
cel_crsf_stream* s = cel_crsf_stream_create();
cel_crsf_frame frames[4];
int n = cel_crsf_stream_feed(s, buf, len1 + len2, frames, 4);
TEST_ASSERT_EQUAL_INT(2, n);
TEST_ASSERT_EQUAL_UINT8(0xC8, frames[0].addr);
TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_HEARTBEAT, frames[0].type);
TEST_ASSERT_EQUAL_UINT8(0xEE, frames[1].addr);
TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_GPS, frames[1].type);
cel_crsf_stream_destroy(s);
}
void test_stream_feed_skip_bad_sync(void) {
uint8_t buf[48];
/* Garbage bytes before valid frame */
buf[0] = 0xFF;
buf[1] = 0xFE;
buf[2] = 0xFD;
uint8_t payload[2] = {0x80, 0x01};
size_t len = build_frame(buf + 3, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, payload, 2);
cel_crsf_stream* s = cel_crsf_stream_create();
cel_crsf_frame frames[4];
int n = cel_crsf_stream_feed(s, buf, 3 + len, frames, 4);
TEST_ASSERT_EQUAL_INT(1, n);
TEST_ASSERT_EQUAL_UINT8(0xC8, frames[0].addr);
cel_crsf_stream_destroy(s);
}
void test_stream_feed_discard_bad_crc(void) {
uint8_t buf[32];
uint8_t payload[2] = {0x80, 0x01};
size_t len = build_frame(buf, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, payload, 2);
buf[len - 1] ^= 0xFF; /* corrupt CRC */
cel_crsf_stream* s = cel_crsf_stream_create();
cel_crsf_frame frames[4];
int n = cel_crsf_stream_feed(s, buf, len, frames, 4);
TEST_ASSERT_EQUAL_INT(0, n); /* bad frame discarded */
cel_crsf_stream_destroy(s);
}
void test_stream_feed_overflow(void) {
uint8_t buf[32];
uint8_t payload[2] = {0x80, 0x01};
size_t len = build_frame(buf, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, payload, 2);
cel_crsf_stream* s = cel_crsf_stream_create();
cel_crsf_frame frames[1];
/* Feed two frames but only have room for one */
size_t total = len * 2;
memcpy(buf + len, buf, len);
int n = cel_crsf_stream_feed(s, buf, total, frames, 1);
TEST_ASSERT_EQUAL_INT(1, n);
/* Second frame should still be in buffer */
n = cel_crsf_stream_feed(s, NULL, 0, frames, 1);
TEST_ASSERT_EQUAL_INT(1, n);
cel_crsf_stream_destroy(s);
}
void test_stream_reset(void) {
uint8_t buf[32];
uint8_t payload[2] = {0x80, 0x01};
size_t len = build_frame(buf, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, payload, 2);
cel_crsf_stream* s = cel_crsf_stream_create();
/* Feed partial frame */
cel_crsf_frame frames[4];
cel_crsf_stream_feed(s, buf, 3, frames, 4);
/* Reset should discard partial */
cel_crsf_stream_reset(s);
/* Feed complete frame — should parse normally */
int n = cel_crsf_stream_feed(s, buf, len, frames, 4);
TEST_ASSERT_EQUAL_INT(1, n);
cel_crsf_stream_destroy(s);
}
int main(void) {
UNITY_BEGIN();
RUN_TEST(test_stream_create_destroy);
RUN_TEST(test_stream_feed_empty);
RUN_TEST(test_stream_feed_single_frame);
RUN_TEST(test_stream_feed_incremental);
RUN_TEST(test_stream_feed_multiple_frames);
RUN_TEST(test_stream_feed_skip_bad_sync);
RUN_TEST(test_stream_feed_discard_bad_crc);
RUN_TEST(test_stream_feed_overflow);
RUN_TEST(test_stream_reset);
return UNITY_END();
}