portersky
cd7d411332
CI / macOS (push) Has been cancelled
CI / Windows / Clang (push) Has been cancelled
113 lines
3.5 KiB
C
113 lines
3.5 KiB
C
#include "celrs/crsf.h"
|
|
#include "celrs/serial.h"
|
|
#include "celrs/logger.h"
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
/* TX power index to dBm mapping (ELRS standard) */
|
|
static int const s_tx_power_dbm[] = {
|
|
0, 20, 26, 30, 32, 34, 36, 38,
|
|
0, 0, 0, 0, 0, 0, 0, 0
|
|
};
|
|
|
|
/* Parse link telemetry payload (5 bytes) from CRSF frame type 0x02 */
|
|
static int telemetry_parse_link(int16_t* rssi, uint8_t* link_quality,
|
|
int8_t* snr, int* tx_power_dbm,
|
|
uint8_t* rssi_rc,
|
|
uint8_t const* payload, size_t len) {
|
|
if (rssi == NULL || payload == NULL) return -1;
|
|
if (len < 5) return -1;
|
|
|
|
*rssi = (int16_t)payload[0]; /* 0-100% */
|
|
*link_quality = payload[1]; /* 0-100% */
|
|
*snr = (int8_t)payload[2]; /* signed dB */
|
|
uint8_t power_idx = payload[3];
|
|
*tx_power_dbm = (power_idx < sizeof(s_tx_power_dbm) / sizeof(s_tx_power_dbm[0]))
|
|
? s_tx_power_dbm[power_idx]
|
|
: 0;
|
|
*rssi_rc = payload[4]; /* 0-100% */
|
|
|
|
return 0;
|
|
}
|
|
|
|
static void print_usage(char const* prog) {
|
|
printf("Usage: %s <serial_port> [interval_ms]\n", prog);
|
|
printf(" serial_port : COM3 (Windows) or /dev/ttyUSB0 (Linux)\n");
|
|
printf(" interval_ms : poll interval in ms (default 200)\n");
|
|
}
|
|
|
|
int main(int argc, char const* argv[]) {
|
|
if (argc < 2) {
|
|
print_usage(argv[0]);
|
|
return 1;
|
|
}
|
|
|
|
char const* port_path = argv[1];
|
|
int interval_ms = 200;
|
|
if (argc >= 3) {
|
|
interval_ms = atoi(argv[2]);
|
|
if (interval_ms <= 0) interval_ms = 200;
|
|
}
|
|
|
|
/* Open serial port */
|
|
cel_serial_port* port = cel_serial_open(port_path, 400000);
|
|
if (port == NULL) {
|
|
cel_log_err("Failed to open serial port");
|
|
return 1;
|
|
}
|
|
|
|
char msg[256];
|
|
snprintf(msg, sizeof(msg), "Connected to %s (400000 baud)", port_path);
|
|
cel_log_info(msg);
|
|
|
|
/* Send heartbeat to establish CRSF link */
|
|
uint8_t hb_payload[2] = {CEL_CRSF_ADDRESS_TBS_GROUND_STATION, 0x01};
|
|
uint8_t hb_frame[256];
|
|
size_t hb_len = cel_crsf_frame_build(hb_frame, CEL_CRSF_ADDRESS_FC_BROADCAST,
|
|
CEL_CRSF_ADDRESS_TBS_GROUND_STATION,
|
|
CEL_CRSF_FRAMETYPE_HEARTBEAT, hb_payload, 2);
|
|
cel_serial_write(port, hb_frame, hb_len);
|
|
|
|
printf("RX\tLINK\tSNR\tTXP\tRSSI_RC\n");
|
|
|
|
/* Read loop */
|
|
uint8_t buf[256];
|
|
int frames = 0, errors = 0;
|
|
|
|
for (int i = 0; i < 20; i++) { /* read up to 20 telemetry frames */
|
|
size_t n = cel_serial_read(port, buf, sizeof(buf), interval_ms);
|
|
if (n == 0) continue;
|
|
|
|
cel_crsf_frame frame;
|
|
if (cel_crsf_frame_parse(&frame, buf, n) != 0) {
|
|
errors++;
|
|
continue;
|
|
}
|
|
|
|
if (frame.type != CEL_CRSF_FRAMETYPE_PACKET_LINK_TELEMETRY) {
|
|
continue; /* skip non-telemetry frames */
|
|
}
|
|
|
|
int16_t rssi;
|
|
uint8_t link_quality;
|
|
int8_t snr;
|
|
int tx_power;
|
|
uint8_t rssi_rc;
|
|
|
|
if (telemetry_parse_link(&rssi, &link_quality, &snr,
|
|
&tx_power, &rssi_rc,
|
|
frame.payload, frame.size) == 0) {
|
|
frames++;
|
|
printf("%d\t%d\t%d\t%d\t%d\n",
|
|
rssi, link_quality, snr, tx_power, rssi_rc);
|
|
}
|
|
}
|
|
|
|
snprintf(msg, sizeof(msg), "Frames: %d, Errors: %d", frames, errors);
|
|
cel_log_info(msg);
|
|
|
|
cel_serial_close(port);
|
|
return 0;
|
|
}
|