diff --git a/celrs/crsf_param.c b/celrs/crsf_param.c index 757fed9..261e740 100644 --- a/celrs/crsf_param.c +++ b/celrs/crsf_param.c @@ -1,7 +1,70 @@ -#include "celrs/crsf_param.h" +#include +#include #include #include +#include "celrs/crsf_param.h" + +/* --------------------------------------------------------------------------- */ +/* Helpers for power matching */ +/* --------------------------------------------------------------------------- */ + +/* Case-insensitive string contains */ +static int str_contains_ci(char const* haystack, char const* needle) { + size_t hlen = strlen(haystack); + size_t nlen = strlen(needle); + if (nlen > hlen) return 0; + for (size_t i = 0; i <= hlen - nlen; i++) { + int match = 1; + for (size_t j = 0; j < nlen; j++) { + if (tolower((unsigned char)haystack[i + j]) + != tolower((unsigned char)needle[j])) + { + match = 0; + break; + } + } + if (match) return 1; + } + return 0; +} + +/* Check if param name contains "power" (case-insensitive) */ +static int is_power_param(cel_crsf_param const* param) { + return str_contains_ci(param->name, "power"); +} + +/* Match requested power (mW) against a TEXT_SELECT option string. + * Returns option index (0-based) or -1 if no match. */ +static int match_power_option(cel_crsf_param const* param, int mw) { + char const* opts = param->options; + char opt_buf[64]; + int index = 0; + + while (*opts) { + /* Extract option up to ';' or '\0' */ + size_t i = 0; + while (*opts && *opts != ';' && i < sizeof(opt_buf) - 1) { + opt_buf[i++] = *opts++; + } + opt_buf[i] = '\0'; + if (*opts == ';') opts++; /* skip separator */ + + /* Check if option starts with the requested mW value */ + char req_buf[16]; + sprintf(req_buf, "%d", mw); + if (str_contains_ci(opt_buf, req_buf)) { + return index; + } + index++; + } + return -1; +} + +/* --------------------------------------------------------------------------- */ +/* Public API */ +/* --------------------------------------------------------------------------- */ + int cel_crsf_param_ping(cel_serial_port* port, float timeout_sec) { if (port == NULL) return -1; @@ -101,22 +164,31 @@ int cel_crsf_param_write(cel_serial_port* port, uint8_t index, int cel_crsf_param_set_power(cel_serial_port* port, int mw, float timeout_sec) { - /* TODO: enumerate parameters until TX Power entry is found. - * 1. Send ping, verify DEVICE_INFO response (call cel_crsf_param_ping). - * 2. For index 0..31: - * a. Send param read frame for index. - * b. Wait for PARAM_ENTRY response (timeout per param). - * c. If no response, stop (end of parameter list). - * d. If param name contains "power" (case-insensitive) - * and type is CEL_PARAM_TEXT_SELECT: - * - Search options for matching power level string - * (e.g. "10" matches "10 mW", "dynamic" matches "dynamic"). - * - Send param write frame with matching option index. - * - Return 0. - * 3. If loop completes without finding power param, return -1. */ - (void)port; - (void)mw; - (void)timeout_sec; + if (port == NULL) return -1; + + /* 1. Ping to verify connection */ + if (cel_crsf_param_ping(port, timeout_sec) != 0) return -1; + + /* 2. Enumerate params until power entry is found */ + for (uint8_t idx = 0; idx < 32; idx++) { + cel_crsf_param param; + if (cel_crsf_param_read(port, idx, ¶m, timeout_sec) != 0) { + /* No response = end of parameter list */ + break; + } + + /* Check if this is the power parameter */ + if (is_power_param(¶m) && param.type == CEL_PARAM_TEXT_SELECT) { + /* Find matching option */ + int opt_index = match_power_option(¶m, mw); + if (opt_index >= 0) { + /* Write the selected option */ + return cel_crsf_param_write(port, idx, (uint8_t)opt_index); + } + } + } + + /* 3. Power param not found */ return -1; } diff --git a/celrs/crsf_param.h b/celrs/crsf_param.h index 87712c0..fba497b 100644 --- a/celrs/crsf_param.h +++ b/celrs/crsf_param.h @@ -1,6 +1,7 @@ #pragma once -#include #include +#include + #include "celrs/crsf.h" #include "celrs/serial.h" diff --git a/tests/test_crsf_param.c b/tests/test_crsf_param.c index 9aca70d..a46d90f 100644 --- a/tests/test_crsf_param.c +++ b/tests/test_crsf_param.c @@ -1,8 +1,9 @@ +#include +#include + #include "unity.h" #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]; @@ -10,11 +11,46 @@ 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 */ +/* Response queue for complex scenarios (set_power) */ +#define MAX_RESPONSES 32 +static uint8_t s_responses[MAX_RESPONSES][260]; +static size_t s_response_lens[MAX_RESPONSES]; +static int s_response_count = 0; +static int s_response_index = 0; +static int s_use_queue = 0; + +/* Write callback to count writes */ +static int s_write_count = 0; + +static size_t mock_write_cb(cel_serial_platform_handle handle, + uint8_t const* buf, size_t len, int call_instance) { + (void)handle; + (void)buf; + (void)call_instance; + s_write_count++; + return len; /* always succeed */ +} + 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++; + + /* Queue-based responses (for set_power tests) */ + if (s_use_queue) { + if (s_response_index < s_response_count) { + size_t to_copy = s_response_lens[s_response_index] < len + ? s_response_lens[s_response_index] : len; + if (to_copy > 0) + memcpy(buf, s_responses[s_response_index], to_copy); + s_response_index++; + return to_copy; + } + return 0; /* exhaust queue -> return 0 (timeout) */ + } + + /* Simple mode */ 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); @@ -26,6 +62,10 @@ void setUp(void) { s_mock_read_calls = 0; s_mock_read_zero_until = 0; s_mock_read_len = 0; + s_use_queue = 0; + s_response_count = 0; + s_response_index = 0; + s_write_count = 0; } void tearDown(void) { @@ -346,6 +386,124 @@ void test_param_read_timeout(void) { cel_serial_close(port); } +/* cel_crsf_param_set_power tests */ + +void test_set_power_null_port(void) { + TEST_ASSERT_EQUAL_INT(-1, cel_crsf_param_set_power(NULL, 100, 1.0f)); +} + +/* Verify build_frame + parse roundtrip works */ +void test_set_power_frame_roundtrip(void) { + uint8_t p1_payload[] = { + 0x10, 0xEE, 0x01, 0x00, 0x00, CEL_PARAM_TEXT_SELECT, + 'T', 'X', ' ', 'P', 'o', 'w', 'e', 'r', '\0', + '1', '0', ' ', 'm', 'W', ';', '1', '0', '0', ' ', 'm', 'W', ';', + 'd', 'y', 'n', 'a', 'm', 'i', 'c', '\0', + 0x00, 0x02, 0x00, 0x00, + }; + size_t frame_len = build_frame(s_mock_read_buf, + CEL_CRSF_TYPE_PARAM_ENTRY, p1_payload, sizeof(p1_payload)); + + /* Parse the frame */ + cel_crsf_frame frame; + int rc = cel_crsf_frame_parse(&frame, s_mock_read_buf, frame_len); + TEST_ASSERT_EQUAL_INT(0, rc); + TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_PARAM_ENTRY, frame.type); + + /* Parse the param */ + cel_crsf_param param; + rc = cel_crsf_param_parse(¶m, frame.payload, frame.payload_len); + TEST_ASSERT_EQUAL_INT(0, rc); + TEST_ASSERT_EQUAL_UINT8(1, param.index); + TEST_ASSERT_EQUAL_STRING("TX Power", param.name); + TEST_ASSERT_EQUAL_UINT8(CEL_PARAM_TEXT_SELECT, param.type); + TEST_ASSERT_NOT_NULL(strstr(param.options, "100 mW")); +} + +void test_set_power_success(void) { + /* Enqueue responses: DEVICE_INFO, PARAM_ENTRY(idx=0), PARAM_ENTRY(idx=1=power) */ + + /* 1. DEVICE_INFO for ping */ + uint8_t di_payload[] = {0x10, 0xEE, 0x00}; + s_response_lens[s_response_count] = + build_frame(s_responses[s_response_count], + CEL_CRSF_TYPE_DEVICE_INFO, di_payload, sizeof(di_payload)); + s_response_count++; + + /* 2. PARAM_ENTRY for index 0 (not power) */ + uint8_t p0_payload[] = { + 0x10, 0xEE, 0x00, 0x00, 0x00, 0x00, + 'R', 'C', '\0', 0x00, 0xFF, 0x80, 0x42, + }; + s_response_lens[s_response_count] = + build_frame(s_responses[s_response_count], + CEL_CRSF_TYPE_PARAM_ENTRY, p0_payload, sizeof(p0_payload)); + s_response_count++; + + /* 3. PARAM_ENTRY for index 1 (TX Power, TEXT_SELECT) */ + uint8_t p1_payload[] = { + 0x10, 0xEE, 0x01, 0x00, 0x00, CEL_PARAM_TEXT_SELECT, + 'T', 'X', ' ', 'P', 'o', 'w', 'e', 'r', '\0', + '1', '0', ' ', 'm', 'W', ';', '1', '0', '0', ' ', 'm', 'W', ';', + 'd', 'y', 'n', 'a', 'm', 'i', 'c', '\0', + 0x00, 0x02, 0x00, 0x00, /* value,min,max,default */ + }; + s_response_lens[s_response_count] = + build_frame(s_responses[s_response_count], + CEL_CRSF_TYPE_PARAM_ENTRY, p1_payload, sizeof(p1_payload)); + s_response_count++; + + s_use_queue = 1; + + 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_StubWithCallback(mock_write_cb); + cel_serial_platform_read_StubWithCallback(mock_read_cb); + + /* Request 100 mW -> should match "100 mW" option (index 1) */ + TEST_ASSERT_EQUAL_INT(0, cel_crsf_param_set_power(port, 100, 1.0f)); + + cel_serial_platform_close_Expect((cel_serial_platform_handle)42); + cel_serial_close(port); +} + +void test_set_power_not_found(void) { + /* Enqueue: DEVICE_INFO, then PARAM_ENTRY for index 0 only */ + uint8_t di_payload[] = {0x10, 0xEE, 0x00}; + s_response_lens[s_response_count] = + build_frame(s_responses[s_response_count], + CEL_CRSF_TYPE_DEVICE_INFO, di_payload, sizeof(di_payload)); + s_response_count++; + + uint8_t p0_payload[] = { + 0x10, 0xEE, 0x00, 0x00, 0x00, 0x00, + 'R', 'C', '\0', 0x00, 0xFF, 0x80, 0x42, + }; + s_response_lens[s_response_count] = + build_frame(s_responses[s_response_count], + CEL_CRSF_TYPE_PARAM_ENTRY, p0_payload, sizeof(p0_payload)); + s_response_count++; + + s_use_queue = 1; + + 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_StubWithCallback(mock_write_cb); + cel_serial_platform_read_StubWithCallback(mock_read_cb); + + /* No power param in the list -> returns -1 after timeout on idx=1 */ + TEST_ASSERT_EQUAL_INT(-1, cel_crsf_param_set_power(port, 100, 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); @@ -367,5 +525,9 @@ int main(void) { RUN_TEST(test_param_read_null_out); RUN_TEST(test_param_read_success); RUN_TEST(test_param_read_timeout); + RUN_TEST(test_set_power_null_port); + RUN_TEST(test_set_power_frame_roundtrip); + RUN_TEST(test_set_power_success); + RUN_TEST(test_set_power_not_found); return UNITY_END(); }