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

@@ -1,19 +1,26 @@
-#include "celrs/crsf.h"
+#include <signal.h>
-#include "celrs/crsf_telemetry.h"
-#include "celrs/serial.h"
-#include "celrs/logger.h"
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <time.h>
+
+#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 <windows.h>
 #else
-#include <time.h>
 #include <unistd.h>
-#include <signal.h>
 #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 <serial_port> [--baudrate <rate>] [interval_ms]\n", prog);
+    printf("Usage: %s [--port <serial_port>] [--baudrate <rate>]\n", prog);
     printf("       %s --list\n", prog);
     printf("  --port <serial_port> : COM3 (Windows) or /dev/ttyUSB0 (Linux)\n");
-    printf("  --baudrate <rate>    : baud rate (default 400000)\n");
+    printf("  --baudrate <rate>    : baud rate (default: auto-probe)\n");
-    printf("  interval_ms          : poll interval in ms (default 200)\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 baud_rate = 0; /* 0 = auto-probe */
-    int interval_ms = 200;
 
     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]);
+        char detected[256];
-        return 1;
+        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 */
+    /* Open serial port with baud probing */
-    cel_serial_port* port = cel_serial_open(port_path, baud_rate);
+    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...");