portersky
d2331229eb
Add baud rate probing (921600/400000/420000), auto-detect ELRS ports, verify module responds to CRSF ping before telemetry loop, and increase RC send rate to 50 Hz to match Python reference.
255 lines
7.5 KiB
C
255 lines
7.5 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
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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) {
|
|
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 */
|
|
uint8_t read_buf[256];
|
|
int rc_count = 0;
|
|
|
|
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);
|
|
|
|
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 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);
|
|
}
|
|
|
|
sleep_ms(20);
|
|
}
|
|
|
|
cel_log_info("Shutting down...");
|
|
cel_crsf_stream_destroy(stream);
|
|
cel_serial_close(port);
|
|
return 0;
|
|
}
|