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.
This commit is contained in:
+77
-15
@@ -1,27 +1,89 @@
|
||||
#include "celrs/crsf_param.h"
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user