celrs/tools/telemetry.c

#include "celrs/crsf.h"
#include "celrs/crsf_telemetry.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>
#include <unistd.h>
#include <signal.h>
#endif

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
}

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

static void print_link_stats(cel_telem_link const* link) {
    printf("[LINK] up=%d/%d rssi %d%% %ddBm "
           "down=%d/%d rssi %ddBm\n",
           link->uplink_rssi1, link->uplink_rssi2,
           link->uplink_quality,
           link->uplink_snr - 128,
           link->downlink_rssi, link->downlink_qual,
           link->downlink_snr - 128);
}

static void print_battery(cel_telem_battery const* bat) {
    printf("[BATT] %.2fV %.2fA %dmah %d%%\n",
           bat->voltage_mv / 1000.0f,
           bat->current_ma / 1000.0f,
           bat->capacity_mah,
           bat->remaining_pct);
}

static void print_heartbeat(cel_telem_heartbeat const* hb) {
    printf("[HB] origin=0x%04X\n", hb->origin_addr);
}

static void print_airspeed(cel_telem_airspeed const* air) {
    printf("[AIR] %.1f km/h\n", air->speed_kmh / 10.0f);
}

static void print_frame_type(uint8_t type) {
    switch (type) {
        case CEL_CRSF_TYPE_LINK_STATS:
            printf("[LINK]\n"); break;
        case CEL_CRSF_TYPE_BATTERY:
            printf("[BATT]\n"); break;
        case CEL_CRSF_TYPE_HEARTBEAT:
            printf("[HB]\n"); break;
        case CEL_CRSF_TYPE_AIRSPEED:
            printf("[AIR]\n"); break;
        default:
            printf("[0x%02X]\n", type); break;
    }
}

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

    setup_signal_handler();

    /* 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);

    /* 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 */
    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));
        if (bytes > 0) {
            cel_crsf_frame frames[4];
            int n = cel_crsf_stream_feed(stream, read_buf, bytes, frames, 4);

            for (int i = 0; i < n; i++) {
                cel_telemetry telem;
                if (cel_crsf_telemetry_parse(&frames[i], &telem) == 0) {
                    switch (telem.type) {
                        case CEL_TELEM_LINK:
                            print_link_stats(&telem.data.link);
                            break;
                        case CEL_TELEM_BATTERY:
                            print_battery(&telem.data.battery);
                            break;
                        case CEL_TELEM_HEARTBEAT:
                            print_heartbeat(&telem.data.heartbeat);
                            break;
                        case CEL_TELEM_AIRSPEED:
                            print_airspeed(&telem.data.airspeed);
                            break;
                        default:
                            print_frame_type(telem.type);
                            break;
                    }
                } else {
                    /* Not a telemetry frame we can decode */
                    print_frame_type(frames[i].type);
                }
            }
        }

        /* Send RC frame periodically to keep link alive */
        rc_count++;
        if (rc_count % 10 == 0) {
            rc_len = cel_crsf_build_rc_frame(rc_buf, channels);
            cel_serial_write(port, rc_buf, rc_len);
        }

        sleep_ms(interval_ms);
    }

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