diff --git a/GOAL.md b/GOAL.md new file mode 100644 index 0000000..d45142c --- /dev/null +++ b/GOAL.md @@ -0,0 +1,7 @@ +Implement all TODOs in the codebase. + +write test first follow @AGENTS.md, commit when implemented one at a time and it passes. + +git push when implemented. + +You need to able to run telemetry on COM14 and start receiving data from TX module. diff --git a/celrs/crsf_telemetry.c b/celrs/crsf_telemetry.c index 8757e15..7e33eea 100644 --- a/celrs/crsf_telemetry.c +++ b/celrs/crsf_telemetry.c @@ -55,6 +55,29 @@ int cel_crsf_telemetry_parse(cel_crsf_frame const* frame, return 0; } + case CEL_CRSF_TYPE_ATTITUDE: { + if (len < 6) return -1; + out->type = CEL_TELEM_ATTITUDE; + /* Each value is int16 big-endian / 10000.0 = radians */ + int16_t pitch = (int16_t)((p[0] << 8) | p[1]); + int16_t roll = (int16_t)((p[2] << 8) | p[3]); + int16_t yaw = (int16_t)((p[4] << 8) | p[5]); + out->data.attitude.pitch_rad = pitch / 10000.0f; + out->data.attitude.roll_rad = roll / 10000.0f; + out->data.attitude.yaw_rad = yaw / 10000.0f; + return 0; + } + + case CEL_CRSF_TYPE_FLIGHT_MODE: { + if (len == 0) return -1; + out->type = CEL_TELEM_FLIGHT_MODE; + size_t copy_len = len < sizeof(out->data.flight_mode.name) ? len + : sizeof(out->data.flight_mode.name); + memcpy(out->data.flight_mode.name, p, copy_len); + out->data.flight_mode.name[copy_len] = 0; + return 0; + } + default: return -1; } diff --git a/tools/telemetry.c b/tools/telemetry.c index 1fde8be..72bb262 100644 --- a/tools/telemetry.c +++ b/tools/telemetry.c @@ -56,6 +56,267 @@ static void sleep_ms(int ms) { #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 ] [--baudrate ]\n", prog); printf(" %s --list\n", prog); @@ -81,47 +342,6 @@ static int list_ports(void) { 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; - } -} - /* 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) { @@ -181,7 +401,7 @@ int main(int argc, char const* argv[]) { /* Verify module responds to CRSF ping */ if (verify_connection(port) != 0) { - cel_log_warn("Continuing anyway — telemetry may not arrive"); + cel_log_warn("Continuing anyway - telemetry may not arrive"); } /* Create CRSF stream for incremental parsing */ @@ -200,7 +420,10 @@ int main(int argc, char const* argv[]) { 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) { @@ -210,29 +433,13 @@ int main(int argc, char const* argv[]) { 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) { - 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; - } + dashboard_update(&dash, &telem, now); } else { - /* Not a telemetry frame we can decode */ - print_frame_type(frames[i].type); + dash.unknown++; } } } @@ -244,6 +451,12 @@ int main(int argc, char const* argv[]) { 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); }