portersky
787a303cf5
Add trailing spaces to shorter status labels so they don't leave residue when overwritten by longer ones. Replace em dashes with regular dashes per project style.
490 lines
14 KiB
C
490 lines
14 KiB
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 - all go to stdout so dashboard owns it */
|
|
/* --------------------------------------------------------------------------- */
|
|
|
|
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_cursor_up(int n) {
|
|
char buf[16];
|
|
snprintf(buf, sizeof(buf), "\033[%dA", n);
|
|
printf("%s", buf);
|
|
}
|
|
|
|
/* --------------------------------------------------------------------------- */
|
|
/* 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 - tracks line count for in-place redraw */
|
|
/* --------------------------------------------------------------------------- */
|
|
|
|
static void render_dashboard(dashboard_t const* d,
|
|
char const* port, int baud,
|
|
double elapsed, int* lines) {
|
|
double now = (double)time(NULL);
|
|
status_t status = compute_status(now, d->link_t, d->fc_t);
|
|
|
|
/* Return to top of dashboard if already drawn */
|
|
if (*lines > 0) {
|
|
ansi_cursor_up(*lines);
|
|
}
|
|
|
|
int n = 0;
|
|
|
|
/* Title */
|
|
ansi_bold();
|
|
printf("CRSF Telemetry");
|
|
ansi_reset();
|
|
printf(" %s @ %d baud up %.0fs\n",
|
|
port, baud, elapsed);
|
|
n++;
|
|
|
|
/* Status */
|
|
print_status_label(status);
|
|
printf("\n");
|
|
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");
|
|
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");
|
|
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");
|
|
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");
|
|
n++;
|
|
|
|
/* Footer */
|
|
ansi_dim();
|
|
printf(" rx=%d ignored=%d", d->rx_frames, d->unknown);
|
|
ansi_reset();
|
|
printf("\n");
|
|
n++;
|
|
|
|
*lines = 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;
|
|
int d_lines = 0; /* dashboard line count for cursor tracking */
|
|
|
|
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, &d_lines);
|
|
}
|
|
|
|
sleep_ms(20);
|
|
}
|
|
|
|
/* Move cursor past dashboard before shutdown message */
|
|
if (d_lines > 0) {
|
|
printf("\n");
|
|
}
|
|
cel_log_info("Shutting down...");
|
|
cel_crsf_stream_destroy(stream);
|
|
cel_serial_close(port);
|
|
return 0;
|
|
}
|