portersky
1db5fdb374
Replace line-by-line telemetry dump with in-place dashboard showing: - Status indicator (LIVE/STALE/NO SIGNAL) - LINK: RSSI1, RSSI2, LQ, SNR, power, mode with color coding - BATT: voltage, current, capacity, percentage - IMU: pitch, roll, yaw in degrees - MODE: flight mode name - Frame counts footer Add ATTITUDE (0x1E) and FLIGHT_MODE (0x21) telemetry parsing. Dashboard redraws every 100ms with ANSI cursor control.
85 lines
3.0 KiB
C
85 lines
3.0 KiB
C
#include "celrs/crsf_telemetry.h"
|
|
#include <string.h>
|
|
|
|
/* Helper: read uint16_t little-endian from buffer */
|
|
static uint16_t read_u16(uint8_t const* buf) {
|
|
return (uint16_t)buf[0] | ((uint16_t)buf[1] << 8);
|
|
}
|
|
|
|
int cel_crsf_telemetry_parse(cel_crsf_frame const* frame,
|
|
cel_telemetry* out) {
|
|
if (frame == NULL || out == NULL) return -1;
|
|
|
|
uint8_t type = frame->type;
|
|
uint8_t const* p = frame->payload;
|
|
uint8_t len = frame->payload_len;
|
|
|
|
switch (type) {
|
|
case CEL_CRSF_TYPE_LINK_STATS: {
|
|
if (len < 10) return -1;
|
|
out->type = CEL_TELEM_LINK;
|
|
out->data.link.uplink_rssi1 = p[0];
|
|
out->data.link.uplink_rssi2 = p[1];
|
|
out->data.link.uplink_quality = p[2];
|
|
out->data.link.uplink_snr = (int8_t)p[3];
|
|
out->data.link.active_antenna = p[4];
|
|
out->data.link.rf_mode = p[5];
|
|
out->data.link.uplink_power = p[6];
|
|
out->data.link.downlink_rssi = p[7];
|
|
out->data.link.downlink_qual = p[8];
|
|
out->data.link.downlink_snr = (int8_t)p[9];
|
|
return 0;
|
|
}
|
|
|
|
case CEL_CRSF_TYPE_BATTERY: {
|
|
if (len < 7) return -1;
|
|
out->type = CEL_TELEM_BATTERY;
|
|
out->data.battery.voltage_mv = read_u16(p);
|
|
out->data.battery.current_ma = read_u16(p + 2);
|
|
out->data.battery.capacity_mah = read_u16(p + 4);
|
|
out->data.battery.remaining_pct = p[6];
|
|
return 0;
|
|
}
|
|
|
|
case CEL_CRSF_TYPE_HEARTBEAT: {
|
|
if (len < 2) return -1;
|
|
out->type = CEL_TELEM_HEARTBEAT;
|
|
out->data.heartbeat.origin_addr = read_u16(p);
|
|
return 0;
|
|
}
|
|
|
|
case CEL_CRSF_TYPE_AIRSPEED: {
|
|
if (len < 2) return -1;
|
|
out->type = CEL_TELEM_AIRSPEED;
|
|
out->data.airspeed.speed_kmh = read_u16(p);
|
|
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;
|
|
}
|
|
}
|