celrs/tools/telemetry.c

#include <signal.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 <unistd.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
BOOL WINAPI handle_ctrl_c(DWORD dwCtrlType) {
    if (dwCtrlType == CTRL_C_EVENT) {
        s_running = 0;
        return TRUE;
    }
    return FALSE;
}
#else
static void handle_sigint(int sig) {
    (void)sig;
    s_running = 0;
}
#endif

static void setup_signal_handler(void) {
#ifdef _WIN32
    SetConsoleCtrlHandler(handle_ctrl_c, TRUE);
#else
    signal(SIGINT, handle_sigint);
#endif
}

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
}

/* --------------------------------------------------------------------------- */
/* ANSI helpers                                                                */
/* --------------------------------------------------------------------------- */

static void ansi_reset(void) { printf("\033[0m"); }
static void ansi_red(void) { printf("\033[31m"); }
static void ansi_green(void) { printf("\033[32m"); }
static void ansi_yellow(void) { printf("\033[33m"); }
static void ansi_bold(void) { printf("\033[1m"); }
static void ansi_dim(void) { printf("\033[2m"); }
static void ansi_clear_line(void) { printf("\033[2K"); }
static void ansi_home(void) { printf("\033[H"); }

/* --------------------------------------------------------------------------- */
/* Status helpers                                                              */
/* --------------------------------------------------------------------------- */

#define LINK_STALE_S 3.0f
#define FC_STALE_S 2.0f

typedef enum {
    STATUS_LIVE,
    STATUS_STALE,
    STATUS_NO_LINK,
    STATUS_NO_SIGNAL
} status_t;

static status_t compute_status(double now, double link_t, double fc_t) {
    if (link_t == 0 || (now - link_t) > LINK_STALE_S)
        return STATUS_NO_SIGNAL;
    if (fc_t > 0 && (now - fc_t) > FC_STALE_S)
        return STATUS_STALE;
    return STATUS_LIVE;
}

static void print_status_label(status_t s) {
    switch (s) {
        case STATUS_LIVE:      ansi_green();  printf("LIVE"); break;
        case STATUS_STALE:     ansi_yellow(); printf("STALE"); break;
        case STATUS_NO_LINK:   ansi_red();    printf("NO LINK"); break;
        case STATUS_NO_SIGNAL: ansi_red();    printf("NO SIGNAL"); break;
    }
    ansi_reset();
}

static void print_age(double now, double t) {
    if (t == 0) { ansi_dim(); printf("waiting..."); ansi_reset(); return; }
    double age = now - t;
    ansi_dim();
    printf(" (%.1fs)", age);
    ansi_reset();
}

/* --------------------------------------------------------------------------- */
/* Dashboard state                                                             */
/* --------------------------------------------------------------------------- */

typedef struct {
    /* Link */
    int has_link;
    uint8_t  up_rssi1, up_rssi2;
    uint8_t  up_lq;
    int8_t   up_snr;
    uint8_t  power_idx;
    uint8_t  rf_mode;
    double   link_t;

    /* Battery */
    int has_batt;
    float  batt_v;
    float  batt_a;
    uint16_t batt_mah;
    uint8_t  batt_pct;
    double   batt_t;

    /* Flight mode */
    int has_mode;
    char mode_name[32];
    double mode_t;

    /* Attitude */
    int has_att;
    float att_pitch, att_roll, att_yaw;
    double att_t;

    /* FC timestamp (youngest of batt/mode/att) */
    double fc_t;

    /* Counts */
    int rx_frames;
    int unknown;
} dashboard_t;

static void dashboard_init(dashboard_t* d) {
    memset(d, 0, sizeof(*d));
}

static void dashboard_update(dashboard_t* d, cel_telemetry const* telem,
                              double now) {
    d->rx_frames++;
    switch (telem->type) {
        case CEL_TELEM_LINK:
            d->has_link = 1;
            d->up_rssi1 = telem->data.link.uplink_rssi1;
            d->up_rssi2 = telem->data.link.uplink_rssi2;
            d->up_lq = telem->data.link.uplink_quality;
            d->up_snr = telem->data.link.uplink_snr - 128;
            d->power_idx = telem->data.link.uplink_power;
            d->rf_mode = telem->data.link.rf_mode;
            d->link_t = now;
            break;
        case CEL_TELEM_BATTERY:
            d->has_batt = 1;
            d->batt_v = telem->data.battery.voltage_mv / 1000.0f;
            d->batt_a = telem->data.battery.current_ma / 1000.0f;
            d->batt_mah = telem->data.battery.capacity_mah;
            d->batt_pct = telem->data.battery.remaining_pct;
            d->batt_t = now;
            if (now > d->fc_t) d->fc_t = now;
            break;
        case CEL_TELEM_FLIGHT_MODE:
            d->has_mode = 1;
            size_t copy_len = sizeof(telem->data.flight_mode.name) < sizeof(d->mode_name)
                ? sizeof(telem->data.flight_mode.name) : sizeof(d->mode_name);
            memcpy(d->mode_name, telem->data.flight_mode.name, copy_len);
            d->mode_name[copy_len - 1] = 0;
            d->mode_t = now;
            if (now > d->fc_t) d->fc_t = now;
            break;
        case CEL_TELEM_ATTITUDE:
            d->has_att = 1;
            d->att_pitch = telem->data.attitude.pitch_rad;
            d->att_roll = telem->data.attitude.roll_rad;
            d->att_yaw = telem->data.attitude.yaw_rad;
            d->att_t = now;
            if (now > d->fc_t) d->fc_t = now;
            break;
        default:
            d->unknown++;
            break;
    }
}

/* Uplink power index -> label */
static char const* power_label(uint8_t idx) {
    static char const* labels[] = {
        "0 mW", "10 mW", "25 mW", "100 mW",
        "500 mW", "1000 mW", "2000 mW", "250 mW", "50 mW"
    };
    return idx < 9 ? labels[idx] : "?";
}

/* Color style for LQ percentage */
static void lq_color(uint8_t lq) {
    if (lq >= 80) ansi_green();
    else if (lq >= 40) ansi_yellow();
    else ansi_red();
}

/* Color style for RSSI dBm (lower = stronger) */
static void rssi_color(double dbm) {
    if (dbm <= 80) ansi_green();
    else if (dbm <= 105) ansi_yellow();
    else ansi_red();
}

/* --------------------------------------------------------------------------- */
/* Dashboard render                                                            */
/* --------------------------------------------------------------------------- */

static void render_dashboard(dashboard_t const* d,
                              char const* port, int baud,
                              double elapsed) {
    double now = (double)time(NULL);
    status_t status = compute_status(now, d->link_t, d->fc_t);

    ansi_home();
    ansi_clear_line();

    /* Title */
    ansi_bold();
    printf("CRSF Telemetry");
    ansi_reset();
    printf("  %s @ %d baud  up %.0fs\n",
           port, baud, elapsed);

    /* Status line */
    print_status_label(status);
    printf("\n");

    /* LINK */
    ansi_bold(); printf("  LINK  "); ansi_reset();
    if (d->has_link) {
        double r1_dbm = d->up_rssi1 / 2.0;
        double r2_dbm = d->up_rssi2 / 2.0;
        rssi_color(r1_dbm);
        printf("RSSI1 -%.1fdBm ", r1_dbm);
        ansi_reset();
        rssi_color(r2_dbm);
        printf("RSSI2 -%.1fdBm ", r2_dbm);
        ansi_reset();
        lq_color(d->up_lq);
        printf("LQ %d%% ", d->up_lq);
        ansi_reset();
        printf("SNR %+ddB ", d->up_snr);
        printf("Pwr=%s Mode=%d", power_label(d->power_idx), d->rf_mode);
        print_age(now, d->link_t);
    } else {
        ansi_dim(); printf("waiting..."); ansi_reset();
    }
    printf("\n");

    /* BATT */
    ansi_bold(); printf("  BATT  "); ansi_reset();
    if (d->has_batt) {
        if (d->batt_v > 3.0f) ansi_green(); else ansi_red();
        printf("%.2fV ", d->batt_v);
        ansi_reset();
        printf("%.1fA %dmah %d%%", d->batt_a, d->batt_mah, d->batt_pct);
        print_age(now, d->batt_t);
    } else {
        ansi_dim(); printf("waiting..."); ansi_reset();
    }
    printf("\n");

    /* IMU */
    ansi_bold(); printf("  IMU   "); ansi_reset();
    if (d->has_att) {
        printf("pitch %+7.1f  roll %+7.1f  yaw %+7.1f deg",
               d->att_pitch * 57.2958f,
               d->att_roll * 57.2958f,
               d->att_yaw * 57.2958f);
        print_age(now, d->att_t);
    } else {
        ansi_dim(); printf("waiting..."); ansi_reset();
    }
    printf("\n");

    /* MODE */
    ansi_bold(); printf("  MODE  "); ansi_reset();
    if (d->has_mode) {
        ansi_green(); printf("%s", d->mode_name); ansi_reset();
        print_age(now, d->mode_t);
    } else {
        ansi_dim(); printf("waiting..."); ansi_reset();
    }
    printf("\n");

    /* Footer */
    ansi_dim();
    printf("  rx=%d  ignored=%d", d->rx_frames, d->unknown);
    ansi_reset();
    printf("\n");

    fflush(stdout);
}

/* --------------------------------------------------------------------------- */
/* Usage / port listing                                                        */
/* --------------------------------------------------------------------------- */

static void print_usage(char const* 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: 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) {
    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;
}

/* 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 = 0; /* 0 = auto-probe */

    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;
        }
    }

    /* Auto-detect port if not specified */
    if (port_path == NULL) {
        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 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, 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) {
        cel_log_err("Failed to create CRSF stream");
        cel_serial_close(port);
        return 1;
    }

    /* Send initial RC frame to establish link */
    int16_t channels[16];
    cel_crsf_channel_default(channels);
    uint8_t rc_buf[32];
    size_t rc_len = cel_crsf_build_rc_frame(rc_buf, channels);
    cel_serial_write(port, rc_buf, rc_len);

    /* Telemetry read loop — 20 ms = 50 Hz RC send rate */
    dashboard_t dash;
    dashboard_init(&dash);
    uint8_t read_buf[256];
    time_t t_start = time(NULL);
    int rc_count = 0;

    while (s_running) {
        /* Read available data */
        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);

            double now = (double)time(NULL);
            for (int i = 0; i < n; i++) {
                cel_telemetry telem;
                if (cel_crsf_telemetry_parse(&frames[i], &telem) == 0) {
                    dashboard_update(&dash, &telem, now);
                } else {
                    dash.unknown++;
                }
            }
        }

        /* Send RC frame every 20 ms (50 Hz) to keep link alive */
        rc_count++;
        if (rc_count % 1 == 0) {
            rc_len = cel_crsf_build_rc_frame(rc_buf, channels);
            cel_serial_write(port, rc_buf, rc_len);
        }

        /* Redraw dashboard every 100 ms */
        if (rc_count % 5 == 0) {
            double elapsed = difftime(time(NULL), t_start);
            render_dashboard(&dash, port_path, actual_baud, elapsed);
        }

        sleep_ms(20);
    }

    cel_log_info("Shutting down...");
    cel_crsf_stream_destroy(stream);
    cel_serial_close(port);
    return 0;
}