From d2331229eb88bd911de05af35a6d374a30630a8b Mon Sep 17 00:00:00 2001 From: portersky <24420859+portersky@users.noreply.github.com> Date: Sun, 14 Jun 2026 22:52:40 +0200 Subject: [PATCH] fix: improve telemetry tool connection reliability Add baud rate probing (921600/400000/420000), auto-detect ELRS ports, verify module responds to CRSF ping before telemetry loop, and increase RC send rate to 50 Hz to match Python reference. --- tools/telemetry.c | 83 +++++++++++++++++++++++++++++++++-------------- 1 file changed, 58 insertions(+), 25 deletions(-) diff --git a/tools/telemetry.c b/tools/telemetry.c index 28b7536..1fde8be 100644 --- a/tools/telemetry.c +++ b/tools/telemetry.c @@ -1,19 +1,26 @@ -#include "celrs/crsf.h" -#include "celrs/crsf_telemetry.h" -#include "celrs/serial.h" -#include "celrs/logger.h" +#include #include #include #include +#include + +#include "celrs/crsf.h" +#include "celrs/crsf_param.h" +#include "celrs/crsf_telemetry.h" +#include "celrs/logger.h" +#include "celrs/serial.h" #ifdef _WIN32 #include #else -#include #include -#include #endif +/* Probe bauds: CP210x chips on ELRS can't hit 921600 exactly. */ +static int const s_probe_bauds[] = {921600, 400000, 420000}; +static int const s_probe_bauds_count = + (int)(sizeof(s_probe_bauds) / sizeof(s_probe_bauds[0])); + static volatile int s_running = 1; #ifdef _WIN32 @@ -43,18 +50,19 @@ static void sleep_ms(int ms) { #ifdef _WIN32 Sleep((DWORD)ms); #else - struct timespec ts = {.tv_sec = ms / 1000, .tv_nsec = (ms % 1000) * 1000000L}; + struct timespec ts = {.tv_sec = ms / 1000, + .tv_nsec = (ms % 1000) * 1000000L}; nanosleep(&ts, NULL); #endif } static void print_usage(char const* prog) { - printf("Usage: %s --port [--baudrate ] [interval_ms]\n", prog); + printf("Usage: %s [--port ] [--baudrate ]\n", prog); printf(" %s --list\n", prog); printf(" --port : COM3 (Windows) or /dev/ttyUSB0 (Linux)\n"); - printf(" --baudrate : baud rate (default 400000)\n"); - printf(" interval_ms : poll interval in ms (default 200)\n"); + printf(" --baudrate : baud rate (default: auto-probe)\n"); printf(" --list : list available serial ports and exit\n"); + printf("\nIf --port is omitted, an ELRS-like port is auto-detected.\n"); } static int list_ports(void) { @@ -114,10 +122,18 @@ static void print_frame_type(uint8_t type) { } } +/* Send a ping and wait for DEVICE_INFO to verify the module responds. */ +static int verify_connection(cel_serial_port* port) { + if (cel_crsf_param_ping(port, 2.0f) != 0) { + cel_log_warn("No DEVICE_INFO response — module may not be connected"); + return -1; + } + return 0; +} + int main(int argc, char const* argv[]) { char const* port_path = NULL; - int baud_rate = 400000; - int interval_ms = 200; + int baud_rate = 0; /* 0 = auto-probe */ for (int i = 1; i < argc; i++) { if (strcmp(argv[i], "--list") == 0) { @@ -127,30 +143,47 @@ int main(int argc, char const* argv[]) { } else if (strcmp(argv[i], "--baudrate") == 0 && i + 1 < argc) { baud_rate = atoi(argv[++i]); if (baud_rate <= 0) baud_rate = 400000; - } else { - interval_ms = atoi(argv[i]); - if (interval_ms <= 0) interval_ms = 200; } } + /* Auto-detect port if not specified */ if (port_path == NULL) { - print_usage(argv[0]); - return 1; + char detected[256]; + if (cel_serial_find_elrs_port(detected, sizeof(detected)) == 0) { + port_path = detected; + cel_log_info("Auto-detected ELRS port"); + } else { + cel_log_err("No ELRS-like port found. Use --list to see ports."); + return 1; + } } setup_signal_handler(); - /* Open serial port */ - cel_serial_port* port = cel_serial_open(port_path, baud_rate); + /* Open serial port with baud probing */ + cel_serial_port* port = NULL; + int actual_baud = 0; + if (baud_rate > 0) { + port = cel_serial_open(port_path, baud_rate); + actual_baud = baud_rate; + } else { + port = cel_serial_open_probe(port_path, s_probe_bauds, + s_probe_bauds_count, &actual_baud); + } if (port == NULL) { cel_log_err("Failed to open serial port"); return 1; } char msg[256]; - snprintf(msg, sizeof(msg), "Connected to %s (%d baud)", port_path, baud_rate); + snprintf(msg, sizeof(msg), "Connected to %s (%d baud)", port_path, actual_baud); cel_log_info(msg); + /* Verify module responds to CRSF ping */ + if (verify_connection(port) != 0) { + cel_log_warn("Continuing anyway — telemetry may not arrive"); + } + /* Create CRSF stream for incremental parsing */ cel_crsf_stream* stream = cel_crsf_stream_create(); if (stream == NULL) { @@ -166,13 +199,13 @@ int main(int argc, char const* argv[]) { size_t rc_len = cel_crsf_build_rc_frame(rc_buf, channels); cel_serial_write(port, rc_buf, rc_len); - /* Telemetry read loop */ + /* Telemetry read loop — 20 ms = 50 Hz RC send rate */ uint8_t read_buf[256]; int rc_count = 0; while (s_running) { /* Read available data */ - int bytes = cel_serial_read(port, read_buf, sizeof(read_buf)); + size_t bytes = cel_serial_read(port, read_buf, sizeof(read_buf)); if (bytes > 0) { cel_crsf_frame frames[4]; int n = cel_crsf_stream_feed(stream, read_buf, bytes, frames, 4); @@ -204,14 +237,14 @@ int main(int argc, char const* argv[]) { } } - /* Send RC frame periodically to keep link alive */ + /* Send RC frame every 20 ms (50 Hz) to keep link alive */ rc_count++; - if (rc_count % 10 == 0) { + if (rc_count % 1 == 0) { rc_len = cel_crsf_build_rc_frame(rc_buf, channels); cel_serial_write(port, rc_buf, rc_len); } - sleep_ms(interval_ms); + sleep_ms(20); } cel_log_info("Shutting down...");