feat: implement cel_crsf_param_set_power
Enumerate parameters until TX Power entry is found, match requested mW against TEXT_SELECT options, and write the selected option index. Added: - str_contains_ci() for case-insensitive substring matching - is_power_param() to detect power-related parameters - match_power_option() to find mW in option strings - 4 new tests: null port, frame roundtrip, success, not found
This commit is contained in:
+89
-17
@@ -1,7 +1,70 @@
|
||||
#include "celrs/crsf_param.h"
|
||||
#include <ctype.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
||||
|
||||
+2
-1
@@ -1,6 +1,7 @@
|
||||
#pragma once
|
||||
#include <stdint.h>
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "celrs/crsf.h"
|
||||
#include "celrs/serial.h"
|
||||
|
||||
|
||||
+164
-2
@@ -1,8 +1,9 @@
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
|
||||
#include "unity.h"
|
||||
#include "celrs/crsf_param.h"
|
||||
#include "Mockserial_internal.h"
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
|
||||
/* 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();
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user