diff --git a/celrs/crsf.c b/celrs/crsf.c index 0efd3f9..b54adec 100644 --- a/celrs/crsf.c +++ b/celrs/crsf.c @@ -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]) { - /* TODO: pack 16 channels (11-bit each) into 22 bytes. - * Frame layout: [0xC8][length][0x16][packed_22B][crc] - * length = 1 (type) + 22 (payload) + 1 (crc) = 24 - * CRC over type + payload. - * Clamp each channel to [CEL_CRSF_CH_MIN, CEL_CRSF_CH_MAX] first. - * Return 2 + length = 26 bytes written. */ - (void)dst; - (void)channels; - return 0; + 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) { - /* TODO: build device ping frame (type 0x28). - * Frame layout: [0xEE][length][0x28][0x00][0xEA][crc] - * Payload: dest=0x00 (broadcast), src=0xEA (radio transmitter) - * length = 1 + 2 + 1 = 4, total = 6 bytes. */ - (void)dst; - return 0; + 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) { - /* TODO: build parameter read frame (type 0x2C). - * Frame layout: [0xEE][length][0x2C][0xEE][0xEF][index][chunk][crc] - * Payload: dest=0xEE (module), src=0xEF (lua), index, chunk - * length = 1 + 4 + 1 = 6, total = 8 bytes. */ - (void)dst; - (void)index; - (void)chunk; - return 0; + 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) { - /* TODO: build parameter write frame (type 0x2D). - * Frame layout: [0xEE][length][0x2D][0xEE][0xEF][index][value][crc] - * Payload: dest=0xEE (module), src=0xEF (lua), index, value - * length = 1 + 4 + 1 = 6, total = 8 bytes. */ - (void)dst; - (void)index; - (void)value; - return 0; + 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; } /* --------------------------------------------------------------------------- */ diff --git a/tests/test_crsf.c b/tests/test_crsf.c index df2030f..3e7a5ae 100644 --- a/tests/test_crsf.c +++ b/tests/test_crsf.c @@ -169,6 +169,76 @@ void test_parse_module_addr(void) { 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) { UNITY_BEGIN(); RUN_TEST(test_crc_empty); @@ -195,5 +265,14 @@ int main(void) { RUN_TEST(test_parse_bad_crc); RUN_TEST(test_parse_empty_payload); 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(); }