portersky/celrs
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
d67d9b29d21e3031d4c2cd0d1e22abf82974823c
celrs/tools/telemetry.c
T
portersky d67d9b29d2 fix: parse CRSF battery as big-endian per protocol spec 
CRSF battery frame is big-endian: voltage(u16 BE 0.1V),
current(u16 BE 0.1A), capacity(u24 BE mAh), remaining(u8 %).

Previous code read little-endian with wrong byte count (7 vs 8)
and wrong scaling (/1000 vs /10), producing 9.98V for a 1S battery.
2026-06-14 23:34:12 +02:00

490 lines
14 KiB
C
Raw Blame History

#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[] = {400000, 420000, 921600};
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 5.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_x10 / 10.0f;
d->batt_a = telem->data.battery.current_x10 / 10.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 %umah %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;
}
Reference in New Issue View Git Blame Copy Permalink