Inital commit

tools/CMakeLists.txt

@@ -0,0 +1,4 @@
+add_executable(tool_telemetry telemetry.c)
+target_include_directories(tool_telemetry PRIVATE "${CMAKE_SOURCE_DIR}")
+target_compile_features(tool_telemetry PRIVATE c_std_23)
+target_link_libraries(tool_telemetry PRIVATE celrs_crsf celrs_serial celrs_logger celrs_log_write)

tools/telemetry.c

@@ -0,0 +1,112 @@
+#include "celrs/crsf.h"
+#include "celrs/serial.h"
+#include "celrs/logger.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+/* TX power index to dBm mapping (ELRS standard) */
+static int const s_tx_power_dbm[] = {
+    0, 20, 26, 30, 32, 34, 36, 38,
+    0, 0, 0, 0, 0, 0, 0, 0
+};
+
+/* Parse link telemetry payload (5 bytes) from CRSF frame type 0x02 */
+static int telemetry_parse_link(int16_t* rssi, uint8_t* link_quality,
+                                int8_t* snr, int* tx_power_dbm,
+                                uint8_t* rssi_rc,
+                                uint8_t const* payload, size_t len) {
+    if (rssi == NULL || payload == NULL) return -1;
+    if (len < 5) return -1;
+
+    *rssi         = (int16_t)payload[0];       /* 0-100% */
+    *link_quality = payload[1];                /* 0-100% */
+    *snr          = (int8_t)payload[2];        /* signed dB */
+    uint8_t power_idx = payload[3];
+    *tx_power_dbm = (power_idx < sizeof(s_tx_power_dbm) / sizeof(s_tx_power_dbm[0]))
+                        ? s_tx_power_dbm[power_idx]
+                        : 0;
+    *rssi_rc      = payload[4];               /* 0-100% */
+
+    return 0;
+}
+
+static void print_usage(char const* prog) {
+    printf("Usage: %s <serial_port> [interval_ms]\n", prog);
+    printf("  serial_port  : COM3 (Windows) or /dev/ttyUSB0 (Linux)\n");
+    printf("  interval_ms  : poll interval in ms (default 200)\n");
+}
+
+int main(int argc, char const* argv[]) {
+    if (argc < 2) {
+        print_usage(argv[0]);
+        return 1;
+    }
+
+    char const* port_path = argv[1];
+    int interval_ms = 200;
+    if (argc >= 3) {
+        interval_ms = atoi(argv[2]);
+        if (interval_ms <= 0) interval_ms = 200;
+    }
+
+    /* Open serial port */
+    cel_serial_port* port = cel_serial_open(port_path, 400000);
+    if (port == NULL) {
+        cel_log_err("Failed to open serial port");
+        return 1;
+    }
+
+    char msg[256];
+    snprintf(msg, sizeof(msg), "Connected to %s (400000 baud)", port_path);
+    cel_log_info(msg);
+
+    /* Send heartbeat to establish CRSF link */
+    uint8_t hb_payload[2] = {CEL_CRSF_ADDRESS_TBS_GROUND_STATION, 0x01};
+    uint8_t hb_frame[256];
+    size_t hb_len = cel_crsf_frame_build(hb_frame, CEL_CRSF_ADDRESS_FC_BROADCAST,
+                                          CEL_CRSF_ADDRESS_TBS_GROUND_STATION,
+                                          CEL_CRSF_FRAMETYPE_HEARTBEAT, hb_payload, 2);
+    cel_serial_write(port, hb_frame, hb_len);
+
+    printf("RX\tLINK\tSNR\tTXP\tRSSI_RC\n");
+
+    /* Read loop */
+    uint8_t buf[256];
+    int frames = 0, errors = 0;
+
+    for (int i = 0; i < 20; i++) { /* read up to 20 telemetry frames */
+        size_t n = cel_serial_read(port, buf, sizeof(buf), interval_ms);
+        if (n == 0) continue;
+
+        cel_crsf_frame frame;
+        if (cel_crsf_frame_parse(&frame, buf, n) != 0) {
+            errors++;
+            continue;
+        }
+
+        if (frame.type != CEL_CRSF_FRAMETYPE_PACKET_LINK_TELEMETRY) {
+            continue; /* skip non-telemetry frames */
+        }
+
+        int16_t rssi;
+        uint8_t link_quality;
+        int8_t snr;
+        int tx_power;
+        uint8_t rssi_rc;
+
+        if (telemetry_parse_link(&rssi, &link_quality, &snr,
+                                 &tx_power, &rssi_rc,
+                                 frame.payload, frame.size) == 0) {
+            frames++;
+            printf("%d\t%d\t%d\t%d\t%d\n",
+                   rssi, link_quality, snr, tx_power, rssi_rc);
+        }
+    }
+
+    snprintf(msg, sizeof(msg), "Frames: %d, Errors: %d", frames, errors);
+    cel_log_info(msg);
+
+    cel_serial_close(port);
+    return 0;
+}