celrs/tools/telemetry.c

#include "celrs/crsf.h"
#include "celrs/serial.h"
#include "celrs/logger.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#ifdef _WIN32
#include <windows.h>
#else
#include <time.h>
#endif

/* 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 sleep_ms(int ms) {
#ifdef _WIN32
    Sleep((DWORD)ms);
#else
    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("       %s --list\n", prog);
    printf("  --port <serial_port> : COM3 (Windows) or /dev/ttyUSB0 (Linux)\n");
    printf("  --baudrate <rate>    : baud rate (default 400000)\n");
    printf("  interval_ms          : poll interval in ms (default 200)\n");
    printf("  --list               : list available serial ports and exit\n");
}

static int list_ports(void) {
    char** ports = NULL;
    int count = cel_serial_list_ports(&ports, 0);
    if (count < 0) {
        cel_log_err("Failed to list serial ports");
        return 1;
    }

    for (int i = 0; i < count; i++) {
        printf("%s\n", ports[i]);
    }

    cel_serial_free_ports(ports, count);
    return 0;
}

int main(int argc, char const* argv[]) {
    char const* port_path = NULL;
    int baud_rate = 400000;
    int interval_ms = 200;

    for (int i = 1; i < argc; i++) {
        if (strcmp(argv[i], "--list") == 0) {
            return list_ports();
        } else if (strcmp(argv[i], "--port") == 0 && i + 1 < argc) {
            port_path = argv[++i];
        } 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;
        }
    }

    if (port_path == NULL) {
        print_usage(argv[0]);
        return 1;
    }

    /* Open serial port */
    cel_serial_port* port = cel_serial_open(port_path, baud_rate);
    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);
    cel_log_info(msg);

    /* TODO: update for new ELRS frame format.
     * New format uses cel_crsf_build_rc_frame() and cel_crsf_stream_feed(). */

    /* Send heartbeat to establish CRSF link (legacy format) */
    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_legacy(hb_frame,
                                          CEL_CRSF_ADDRESS_FC_BROADCAST,
                                          CEL_CRSF_ADDRESS_TBS_GROUND_STATION,
                                          0x03, 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));
        if (n == 0) {
            sleep_ms(interval_ms);
            continue;
        }

        cel_crsf_frame_legacy frame;
        if (cel_crsf_frame_parse_legacy(&frame, buf, n) != 0) {
            errors++;
            continue;
        }

        if (frame.type != 0x02) {
            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;
}