From 7b3905084e86ba5371b2c90f4f65ddcb188dd8cd Mon Sep 17 00:00:00 2001 From: portersky <24420859+portersky@users.noreply.github.com> Date: Sun, 14 Jun 2026 22:08:51 +0200 Subject: [PATCH] feat: implement cel_crsf_param_ping and cel_crsf_param_read Ping sends DEVICE_PING frame and waits for DEVICE_INFO response. Read sends PARAM_READ frame and waits for matching PARAM_ENTRY. Both use cel_crsf_stream_feed() with a clock-based timeout loop. --- celrs/crsf_param.c | 92 ++++++++++++++++++++---- tests/cmock.yml | 1 + tests/test_crsf_param.c | 153 ++++++++++++++++++++++++++++++++++++++++ 3 files changed, 231 insertions(+), 15 deletions(-) diff --git a/celrs/crsf_param.c b/celrs/crsf_param.c index 1f06107..757fed9 100644 --- a/celrs/crsf_param.c +++ b/celrs/crsf_param.c @@ -1,27 +1,89 @@ #include "celrs/crsf_param.h" #include +#include int cel_crsf_param_ping(cel_serial_port* port, float timeout_sec) { - /* TODO: send ping frame, wait for DEVICE_INFO (0x29) response. - * Use cel_crsf_build_ping_frame() + cel_serial_write(). - * Read loop with timeout using cel_serial_read(). - * Use cel_crsf_stream_feed() to parse responses. - * Return 0 when DEVICE_INFO received, -1 on timeout. */ - (void)port; - (void)timeout_sec; + if (port == NULL) return -1; + + /* Send ping frame */ + uint8_t frame[16]; + size_t len = cel_crsf_build_ping_frame(frame); + if (len == 0) return -1; + + size_t written = cel_serial_write(port, frame, len); + if (written != len) return -1; + + /* Wait for DEVICE_INFO response */ + cel_crsf_stream* stream = cel_crsf_stream_create(); + if (stream == NULL) return -1; + + clock_t start = clock(); + clock_t limit = (clock_t)(timeout_sec * CLOCKS_PER_SEC); + + while ((clock() - start) < limit) { + uint8_t buf[256]; + size_t n = cel_serial_read(port, buf, sizeof(buf)); + if (n > 0) { + cel_crsf_frame frames[4]; + int count = cel_crsf_stream_feed(stream, buf, n, frames, sizeof(frames) / sizeof(frames[0])); + if (count > 0) { + for (int i = 0; i < count; i++) { + if (frames[i].type == CEL_CRSF_TYPE_DEVICE_INFO) { + cel_crsf_stream_destroy(stream); + return 0; + } + } + } + } + } + + cel_crsf_stream_destroy(stream); return -1; } int cel_crsf_param_read(cel_serial_port* port, uint8_t index, cel_crsf_param* out, float timeout_sec) { - /* TODO: send param read frame for index, wait for PARAM_ENTRY (0x2B). - * Use cel_crsf_build_param_read_frame() + cel_serial_write(). - * Read loop with timeout, parse with cel_crsf_stream_feed(). - * When PARAM_ENTRY with matching index arrives, parse payload and return. */ - (void)port; - (void)index; - (void)out; - (void)timeout_sec; + if (port == NULL || out == NULL) return -1; + + /* Send param read frame */ + uint8_t frame[16]; + size_t len = cel_crsf_build_param_read_frame(frame, index, 0); + if (len == 0) return -1; + + size_t written = cel_serial_write(port, frame, len); + if (written != len) return -1; + + /* Wait for PARAM_ENTRY response with matching index */ + cel_crsf_stream* stream = cel_crsf_stream_create(); + if (stream == NULL) return -1; + + clock_t start = clock(); + clock_t limit = (clock_t)(timeout_sec * CLOCKS_PER_SEC); + + while ((clock() - start) < limit) { + uint8_t buf[256]; + size_t n = cel_serial_read(port, buf, sizeof(buf)); + if (n > 0) { + cel_crsf_frame frames[4]; + int count = cel_crsf_stream_feed(stream, buf, n, frames, sizeof(frames) / sizeof(frames[0])); + if (count > 0) { + for (int i = 0; i < count; i++) { + if (frames[i].type == CEL_CRSF_TYPE_PARAM_ENTRY) { + if (cel_crsf_param_parse(out, frames[i].payload, + frames[i].payload_len) == 0) + { + if (out->index == index) { + cel_crsf_stream_destroy(stream); + return 0; + } + } + } + } + } + } + } + + cel_crsf_stream_destroy(stream); return -1; } diff --git a/tests/cmock.yml b/tests/cmock.yml index 867184a..b9af567 100644 --- a/tests/cmock.yml +++ b/tests/cmock.yml @@ -5,3 +5,4 @@ :plugins: - :ignore_arg - :expect_any_args + - :callback diff --git a/tests/test_crsf_param.c b/tests/test_crsf_param.c index 846ce39..9aca70d 100644 --- a/tests/test_crsf_param.c +++ b/tests/test_crsf_param.c @@ -2,9 +2,30 @@ #include "celrs/crsf_param.h" #include "Mockserial_internal.h" #include +#include + +/* Global state for mock read callbacks */ +static uint8_t s_mock_read_buf[260]; +static size_t s_mock_read_len = 0; +static int s_mock_read_calls = 0; +static int s_mock_read_zero_until = 0; /* return 0 for first N calls */ + +static size_t mock_read_cb(cel_serial_platform_handle handle, + uint8_t* buf, size_t len, int call_instance) { + (void)handle; + (void)call_instance; + s_mock_read_calls++; + if (s_mock_read_calls <= s_mock_read_zero_until) return 0; + size_t to_copy = s_mock_read_len < len ? s_mock_read_len : len; + if (to_copy > 0) memcpy(buf, s_mock_read_buf, to_copy); + return to_copy; +} void setUp(void) { Mockserial_internal_Init(); + s_mock_read_calls = 0; + s_mock_read_zero_until = 0; + s_mock_read_len = 0; } void tearDown(void) { @@ -12,6 +33,19 @@ void tearDown(void) { Mockserial_internal_Destroy(); } +/* Helper: build a valid CRSF frame (payload includes dest+src+data) */ +static size_t build_frame(uint8_t* dst, uint8_t type, + uint8_t const* payload, uint8_t payload_len) { + uint8_t length = 1 + payload_len + 1; + dst[0] = CEL_CRSF_FRAME_HEADER; + 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; +} + /* cel_crsf_param_parse tests */ void test_param_parse_null_args(void) { @@ -200,6 +234,118 @@ void test_param_write_partial_write(void) { cel_serial_close(port); } +/* cel_crsf_param_ping tests */ + +void test_param_ping_null_port(void) { + TEST_ASSERT_EQUAL_INT(-1, cel_crsf_param_ping(NULL, 1.0f)); +} + +void test_param_ping_success(void) { + /* Build a DEVICE_INFO frame (type 0x29) */ + uint8_t payload[] = {0x10, 0xEE, 0x00}; /* dest, src, type */ + size_t frame_len = build_frame(s_mock_read_buf, + CEL_CRSF_TYPE_DEVICE_INFO, payload, sizeof(payload)); + s_mock_read_len = frame_len; + s_mock_read_zero_until = 0; + + cel_serial_platform_open_ExpectAndReturn("COM3", 400000, + (cel_serial_platform_handle)42); + cel_serial_port* port = cel_serial_open("COM3", 400000); + TEST_ASSERT_NOT_NULL(port); + + cel_serial_platform_write_ExpectAnyArgsAndReturn(6); + cel_serial_platform_read_StubWithCallback(mock_read_cb); + + TEST_ASSERT_EQUAL_INT(0, cel_crsf_param_ping(port, 1.0f)); + + cel_serial_platform_close_Expect((cel_serial_platform_handle)42); + cel_serial_close(port); +} + +void test_param_ping_timeout(void) { + cel_serial_platform_open_ExpectAndReturn("COM3", 400000, + (cel_serial_platform_handle)42); + cel_serial_port* port = cel_serial_open("COM3", 400000); + TEST_ASSERT_NOT_NULL(port); + + cel_serial_platform_write_ExpectAnyArgsAndReturn(6); + cel_serial_platform_read_StubWithCallback(mock_read_cb); + /* mock_read_cb always returns 0 since s_mock_read_zero_until defaults to 0 + * and s_mock_read_len is 0 */ + + /* Use a short timeout so test doesn't hang */ + TEST_ASSERT_EQUAL_INT(-1, cel_crsf_param_ping(port, 0.05f)); + + cel_serial_platform_close_Expect((cel_serial_platform_handle)42); + cel_serial_close(port); +} + +/* cel_crsf_param_read tests */ + +void test_param_read_null_port(void) { + cel_crsf_param param; + TEST_ASSERT_EQUAL_INT(-1, cel_crsf_param_read(NULL, 0, ¶m, 1.0f)); +} + +void test_param_read_null_out(void) { + cel_serial_platform_open_ExpectAndReturn("COM3", 400000, + (cel_serial_platform_handle)42); + cel_serial_port* port = cel_serial_open("COM3", 400000); + TEST_ASSERT_NOT_NULL(port); + + /* No write expected since out is NULL */ + TEST_ASSERT_EQUAL_INT(-1, cel_crsf_param_read(port, 5, NULL, 1.0f)); + + cel_serial_platform_close_Expect((cel_serial_platform_handle)42); + cel_serial_close(port); +} + +void test_param_read_success(void) { + /* Build a PARAM_ENTRY frame (type 0x2B) matching index 5 */ + uint8_t payload[] = { + 0x10, 0xEE, 0x05, 0x00, 0x00, 0x00, /* dest,src,idx,chunks,parent,type */ + 'V', 'a', 'l', '\0', /* name */ + 0x00, 0xFF, 0x80, 0x42, /* min,max,default,value */ + }; + size_t frame_len = build_frame(s_mock_read_buf, + CEL_CRSF_TYPE_PARAM_ENTRY, payload, sizeof(payload)); + s_mock_read_len = frame_len; + s_mock_read_zero_until = 0; + + cel_serial_platform_open_ExpectAndReturn("COM3", 400000, + (cel_serial_platform_handle)42); + cel_serial_port* port = cel_serial_open("COM3", 400000); + TEST_ASSERT_NOT_NULL(port); + + cel_serial_platform_write_ExpectAnyArgsAndReturn(8); + cel_serial_platform_read_StubWithCallback(mock_read_cb); + + cel_crsf_param param; + TEST_ASSERT_EQUAL_INT(0, cel_crsf_param_read(port, 5, ¶m, 1.0f)); + TEST_ASSERT_EQUAL_UINT8(5, param.index); + TEST_ASSERT_EQUAL_STRING("Val", param.name); + TEST_ASSERT_EQUAL_UINT8(0x42, param.value); + + cel_serial_platform_close_Expect((cel_serial_platform_handle)42); + cel_serial_close(port); +} + +void test_param_read_timeout(void) { + cel_serial_platform_open_ExpectAndReturn("COM3", 400000, + (cel_serial_platform_handle)42); + cel_serial_port* port = cel_serial_open("COM3", 400000); + TEST_ASSERT_NOT_NULL(port); + + cel_serial_platform_write_ExpectAnyArgsAndReturn(8); + cel_serial_platform_read_StubWithCallback(mock_read_cb); + + cel_crsf_param param; + TEST_ASSERT_EQUAL_INT(-1, cel_crsf_param_read(port, 5, ¶m, 0.05f)); + + cel_serial_platform_close_Expect((cel_serial_platform_handle)42); + cel_serial_close(port); +} + int main(void) { UNITY_BEGIN(); RUN_TEST(test_param_parse_null_args); @@ -214,5 +360,12 @@ int main(void) { RUN_TEST(test_param_write_null_port); RUN_TEST(test_param_write_success); RUN_TEST(test_param_write_partial_write); + RUN_TEST(test_param_ping_null_port); + RUN_TEST(test_param_ping_success); + RUN_TEST(test_param_ping_timeout); + RUN_TEST(test_param_read_null_port); + RUN_TEST(test_param_read_null_out); + RUN_TEST(test_param_read_success); + RUN_TEST(test_param_read_timeout); return UNITY_END(); }