diff --git a/tools/telemetry.c b/tools/telemetry.c index 55a1f74..df16a98 100644 --- a/tools/telemetry.c +++ b/tools/telemetry.c @@ -17,7 +17,7 @@ #endif /* Probe bauds: CP210x chips on ELRS can't hit 921600 exactly. */ -static int const s_probe_bauds[] = {400000, 420000, 921600}; +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])); @@ -88,9 +88,12 @@ typedef enum { STATUS_NO_SIGNAL } status_t; -static status_t compute_status(double now, double link_t, double fc_t) { +static status_t compute_status(double now, double link_t, double fc_t, + int has_link, uint8_t up_lq) { if (link_t == 0 || (now - link_t) > LINK_STALE_S) return STATUS_NO_SIGNAL; + if (has_link && up_lq == 0) + return STATUS_NO_LINK; if (fc_t > 0 && (now - fc_t) > FC_STALE_S) return STATUS_STALE; return STATUS_LIVE; @@ -152,6 +155,7 @@ typedef struct { /* Counts */ int rx_frames; int unknown; + uint32_t type_counts[256]; /* indexed by raw CRSF frame type byte */ } dashboard_t; static void dashboard_init(dashboard_t* d) { @@ -167,7 +171,7 @@ static void dashboard_update(dashboard_t* d, cel_telemetry const* telem, 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->up_snr = telem->data.link.uplink_snr; d->power_idx = telem->data.link.uplink_power; d->rf_mode = telem->data.link.rf_mode; d->link_t = now; @@ -227,6 +231,34 @@ static void rssi_color(double dbm) { else ansi_red(); } +/* Raw CRSF frame type byte -> name, for diagnostics. + Returns NULL for types not in cel_crsf_type. */ +static char const* crsf_type_name(uint8_t type) { + switch (type) { + case CEL_CRSF_TYPE_GPS: return "GPS"; + case CEL_CRSF_TYPE_VARIO: return "VARIO"; + case CEL_CRSF_TYPE_BATTERY: return "BATTERY"; + case CEL_CRSF_TYPE_BARO_ALT: return "BARO_ALT"; + case CEL_CRSF_TYPE_AIRSPEED: return "AIRSPEED"; + case CEL_CRSF_TYPE_HEARTBEAT: return "HEARTBEAT"; + case CEL_CRSF_TYPE_RPM: return "RPM"; + case CEL_CRSF_TYPE_TEMP: return "TEMP"; + case CEL_CRSF_TYPE_VOLTAGES: return "VOLTAGES"; + case CEL_CRSF_TYPE_ESC_SENSOR: return "ESC_SENSOR"; + case CEL_CRSF_TYPE_LINK_STATS: return "LINK_STATS"; + case CEL_CRSF_TYPE_RC_CHANNELS: return "RC_CHANNELS"; + case CEL_CRSF_TYPE_ATTITUDE: return "ATTITUDE"; + case CEL_CRSF_TYPE_FLIGHT_MODE: return "FLIGHT_MODE"; + case CEL_CRSF_TYPE_DEVICE_PING: return "DEVICE_PING"; + case CEL_CRSF_TYPE_DEVICE_INFO: return "DEVICE_INFO"; + case CEL_CRSF_TYPE_PARAM_ENTRY: return "PARAM_ENTRY"; + case CEL_CRSF_TYPE_PARAM_READ: return "PARAM_READ"; + case CEL_CRSF_TYPE_PARAM_WRITE: return "PARAM_WRITE"; + case CEL_CRSF_TYPE_ELRS_STATUS: return "ELRS_STATUS"; + default: return NULL; + } +} + /* --------------------------------------------------------------------------- */ /* Dashboard render - tracks line count for in-place redraw */ /* --------------------------------------------------------------------------- */ @@ -235,7 +267,8 @@ 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); + status_t status = compute_status(now, d->link_t, d->fc_t, + d->has_link, d->up_lq); /* Return to top of dashboard if already drawn */ if (*lines > 0) { @@ -326,6 +359,36 @@ static void render_dashboard(dashboard_t const* d, printf("\n"); n++; + /* Frame type breakdown (top 6 by count) */ + ansi_clear_line(); + ansi_dim(); + printf(" types:"); + uint8_t used[256] = {0}; + int shown = 0; + for (int k = 0; k < 6; k++) { + int best = -1; + uint32_t best_count = 0; + for (int t = 0; t < 256; t++) { + if (!used[t] && d->type_counts[t] > best_count) { + best_count = d->type_counts[t]; + best = t; + } + } + if (best < 0) break; + used[best] = 1; + char const* name = crsf_type_name((uint8_t)best); + if (name != NULL) { + printf(" %s=%u", name, best_count); + } else { + printf(" 0x%02X=%u", best, best_count); + } + shown++; + } + if (shown == 0) printf(" -"); + ansi_reset(); + printf("\n"); + n++; + *lines = n; fflush(stdout); } @@ -361,11 +424,13 @@ static int list_ports(void) { /* 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; + /* The module may take a few seconds to respond to the first ping, + * so retry a few times before giving up. */ + for (int attempt = 0; attempt < 3; attempt++) { + if (cel_crsf_param_ping(port, 2.0f) == 0) return 0; } - return 0; + cel_log_warn("No DEVICE_INFO response - module may not be connected"); + return -1; } int main(int argc, char const* argv[]) { @@ -453,6 +518,7 @@ int main(int argc, char const* argv[]) { double now = (double)time(NULL); for (int i = 0; i < n; i++) { + dash.type_counts[frames[i].type]++; cel_telemetry telem; if (cel_crsf_telemetry_parse(&frames[i], &telem) == 0) { dashboard_update(&dash, &telem, now); @@ -469,6 +535,14 @@ int main(int argc, char const* argv[]) { cel_serial_write(port, rc_buf, rc_len); } + /* Send DEVICE_PING every 5s so DEVICE_INFO replies keep showing up + * in the dashboard's type_counts, like the Python ping_loop. */ + if (rc_count % 250 == 0) { + uint8_t ping_buf[8]; + size_t ping_len = cel_crsf_build_ping_frame(ping_buf); + cel_serial_write(port, ping_buf, ping_len); + } + /* Redraw dashboard every 100 ms */ if (rc_count % 5 == 0) { double elapsed = difftime(time(NULL), t_start);