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

feat: implement frame builders

Browse Source 
cel_crsf_build_rc_frame() packs 16 channels (11-bit) into 22 bytes.
cel_crsf_build_ping_frame() builds device ping (0x28).
cel_crsf_build_param_read_frame() and cel_crsf_build_param_write_frame()
build parameter protocol frames (0x2C/0x2D).
This commit is contained in:
portersky
2026-06-14 20:58:16 +02:00
parent 8b181d0fcd
commit b97a7c5b3a
2 changed files with 149 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
celrs/crsf.c
+70 -31
View File
@@ -35,48 +35,87 @@ uint8_t cel_crsf_crc(uint8_t const* data, size_t len) {
/* --------------------------------------------------------------------------- */ /* --------------------------------------------------------------------------- */
size_t cel_crsf_build_rc_frame(uint8_t* dst, int16_t const channels[16]) { size_t cel_crsf_build_rc_frame(uint8_t* dst, int16_t const channels[16]) {
/* TODO: pack 16 channels (11-bit each) into 22 bytes. if (dst == NULL) return 0;
* Frame layout: [0xC8][length][0x16][packed_22B][crc]
* length = 1 (type) + 22 (payload) + 1 (crc) = 24 /* Pack 16 channels (11-bit each) into 22 bytes */
* CRC over type + payload. int16_t ch[16];
* Clamp each channel to [CEL_CRSF_CH_MIN, CEL_CRSF_CH_MAX] first. if (channels != NULL) {
* Return 2 + length = 26 bytes written. */ memcpy(ch, channels, sizeof(ch));
(void)dst; } else {
(void)channels; memset(ch, 0, sizeof(ch));
return 0; }
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) { size_t cel_crsf_build_ping_frame(uint8_t* dst) {
/* TODO: build device ping frame (type 0x28). if (dst == NULL) return 0;
* Frame layout: [0xEE][length][0x28][0x00][0xEA][crc]
* Payload: dest=0x00 (broadcast), src=0xEA (radio transmitter) uint8_t length = 1 + 2 + 1; /* type + payload(2) + crc */
* length = 1 + 2 + 1 = 4, total = 6 bytes. */ dst[0] = CEL_CRSF_ADDRESS_MODULE;
(void)dst; dst[1] = length;
return 0; 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, size_t cel_crsf_build_param_read_frame(uint8_t* dst, uint8_t index,
uint8_t chunk) { uint8_t chunk) {
/* TODO: build parameter read frame (type 0x2C). if (dst == NULL) return 0;
* Frame layout: [0xEE][length][0x2C][0xEE][0xEF][index][chunk][crc]
* Payload: dest=0xEE (module), src=0xEF (lua), index, chunk uint8_t length = 1 + 4 + 1; /* type + payload(4) + crc */
* length = 1 + 4 + 1 = 6, total = 8 bytes. */ dst[0] = CEL_CRSF_ADDRESS_MODULE;
(void)dst; dst[1] = length;
(void)index; dst[2] = CEL_CRSF_TYPE_PARAM_READ;
(void)chunk; dst[3] = CEL_CRSF_ADDRESS_MODULE; /* dest */
return 0; 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, size_t cel_crsf_build_param_write_frame(uint8_t* dst, uint8_t index,
uint8_t value) { uint8_t value) {
/* TODO: build parameter write frame (type 0x2D). if (dst == NULL) return 0;
* Frame layout: [0xEE][length][0x2D][0xEE][0xEF][index][value][crc]
* Payload: dest=0xEE (module), src=0xEF (lua), index, value uint8_t length = 1 + 4 + 1; /* type + payload(4) + crc */
* length = 1 + 4 + 1 = 6, total = 8 bytes. */ dst[0] = CEL_CRSF_ADDRESS_MODULE;
(void)dst; dst[1] = length;
(void)index; dst[2] = CEL_CRSF_TYPE_PARAM_WRITE;
(void)value; dst[3] = CEL_CRSF_ADDRESS_MODULE; /* dest */
return 0; 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;
} }
/* --------------------------------------------------------------------------- */ /* --------------------------------------------------------------------------- */
tests/test_crsf.c
+79
View File
@@ -169,6 +169,76 @@ void test_parse_module_addr(void) {
TEST_ASSERT_EQUAL_UINT8(4, frame.payload_len); TEST_ASSERT_EQUAL_UINT8(4, frame.payload_len);
} }
/* Frame builder tests */
void test_build_rc_frame_null_dst(void) {
int16_t ch[16];
TEST_ASSERT_EQUAL_UINT(0, cel_crsf_build_rc_frame(NULL, ch));
}
void test_build_rc_frame_null_channels(void) {
uint8_t dst[32];
size_t len = cel_crsf_build_rc_frame(dst, NULL);
TEST_ASSERT_GREATER_THAN(0, len);
TEST_ASSERT_EQUAL_UINT8(0xC8, dst[0]);
}
void test_build_rc_frame_roundtrip(void) {
int16_t ch[16];
cel_crsf_channel_default(ch);
ch[0] = CEL_CRSF_CH_MAX;
uint8_t dst[32];
size_t len = cel_crsf_build_rc_frame(dst, ch);
cel_crsf_frame frame;
TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, dst, len));
TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_RC_CHANNELS_PACKED, frame.type);
}
void test_build_ping_frame_null_dst(void) {
TEST_ASSERT_EQUAL_UINT(0, cel_crsf_build_ping_frame(NULL));
}
void test_build_ping_frame_valid(void) {
uint8_t dst[16];
size_t len = cel_crsf_build_ping_frame(dst);
TEST_ASSERT_GREATER_THAN(0, len);
TEST_ASSERT_EQUAL_UINT8(0xEE, dst[0]);
cel_crsf_frame frame;
TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, dst, len));
TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_DEVICE_PING, frame.type);
}
void test_build_param_read_frame_null_dst(void) {
TEST_ASSERT_EQUAL_UINT(0, cel_crsf_build_param_read_frame(NULL, 0, 0));
}
void test_build_param_read_frame_valid(void) {
uint8_t dst[16];
size_t len = cel_crsf_build_param_read_frame(dst, 0x42, 0);
TEST_ASSERT_GREATER_THAN(0, len);
cel_crsf_frame frame;
TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, dst, len));
TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_PARAM_READ, frame.type);
}
void test_build_param_write_frame_null_dst(void) {
TEST_ASSERT_EQUAL_UINT(0, cel_crsf_build_param_write_frame(NULL, 0, 0));
}
void test_build_param_write_frame_valid(void) {
uint8_t dst[16];
size_t len = cel_crsf_build_param_write_frame(dst, 0x42, 0xFF);
TEST_ASSERT_GREATER_THAN(0, len);
cel_crsf_frame frame;
TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, dst, len));
TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_PARAM_WRITE, frame.type);
}
int main(void) { int main(void) {
UNITY_BEGIN(); UNITY_BEGIN();
RUN_TEST(test_crc_empty); RUN_TEST(test_crc_empty);
@@ -195,5 +265,14 @@ int main(void) {
RUN_TEST(test_parse_bad_crc); RUN_TEST(test_parse_bad_crc);
RUN_TEST(test_parse_empty_payload); RUN_TEST(test_parse_empty_payload);
RUN_TEST(test_parse_module_addr); RUN_TEST(test_parse_module_addr);
RUN_TEST(test_build_rc_frame_null_dst);
RUN_TEST(test_build_rc_frame_null_channels);
RUN_TEST(test_build_rc_frame_roundtrip);
RUN_TEST(test_build_ping_frame_null_dst);
RUN_TEST(test_build_ping_frame_valid);
RUN_TEST(test_build_param_read_frame_null_dst);
RUN_TEST(test_build_param_read_frame_valid);
RUN_TEST(test_build_param_write_frame_null_dst);
RUN_TEST(test_build_param_write_frame_valid);
return UNITY_END(); return UNITY_END();
} }