feat: implement telemetry parser
cel_crsf_telemetry_parse() decodes link stats, battery, heartbeat, and airspeed frames. Updated cel_telem_battery and cel_telem_airspeed structs to use uint16_t values matching CRSF protocol format.
This commit is contained in:
+46
-149
@@ -1,164 +1,61 @@
|
||||
#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;
|
||||
size_t len = frame->payload_len;
|
||||
uint8_t len = frame->payload_len;
|
||||
|
||||
switch (frame->type) {
|
||||
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_GPS:
|
||||
/* TODO: parse GPS payload (15 bytes minimum).
|
||||
* Layout:
|
||||
* [0..3] latitude (int32 big-endian, divide by 1e7)
|
||||
* [4..7] longitude (int32 big-endian, divide by 1e7)
|
||||
* [8..9] groundspeed (uint16 big-endian, divide by 10.0, km/h)
|
||||
* [10..11] heading (uint16 big-endian, divide by 100.0, deg)
|
||||
* [12..13] altitude (uint16 big-endian, subtract 1000, m)
|
||||
* [14] satellites (uint8) */
|
||||
if (len < 15) return -1;
|
||||
memset(out, 0, sizeof(*out));
|
||||
out->type = CEL_TELEM_GPS;
|
||||
break;
|
||||
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_VARIO:
|
||||
/* TODO: parse vario payload (2 bytes).
|
||||
* Layout: [0..1] vertical_speed (int16 big-endian, cm/s) */
|
||||
if (len < 2) return -1;
|
||||
memset(out, 0, sizeof(*out));
|
||||
out->type = CEL_TELEM_VARIO;
|
||||
break;
|
||||
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_BATTERY:
|
||||
/* TODO: parse battery payload (8 bytes).
|
||||
* Layout:
|
||||
* [0..1] voltage (uint16 big-endian mV, divide by 10.0)
|
||||
* [2..3] current (uint16 big-endian mA, divide by 10.0)
|
||||
* [4..6] capacity (uint24 big-endian, mAh)
|
||||
* [7] remaining (uint8, percent) */
|
||||
if (len < 8) return -1;
|
||||
memset(out, 0, sizeof(*out));
|
||||
out->type = CEL_TELEM_BATTERY;
|
||||
break;
|
||||
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_BARO_ALT:
|
||||
/* TODO: parse baro payload (2 or 4 bytes).
|
||||
* Layout:
|
||||
* [0..1] altitude (uint16 big-endian).
|
||||
* If MSB set: altitude = raw & 0x7FFF (meters)
|
||||
* Else: altitude = (raw - 10000) / 10.0 (meters)
|
||||
* [2..3] vertical_speed (int16 big-endian, cm/s) — optional */
|
||||
if (len < 2) return -1;
|
||||
memset(out, 0, sizeof(*out));
|
||||
out->type = CEL_TELEM_BARO;
|
||||
break;
|
||||
|
||||
case CEL_CRSF_TYPE_AIRSPEED:
|
||||
/* TODO: parse airspeed payload (2 bytes).
|
||||
* Layout: [0..1] speed (uint16 big-endian, divide by 10.0, km/h) */
|
||||
if (len < 2) return -1;
|
||||
memset(out, 0, sizeof(*out));
|
||||
out->type = CEL_TELEM_AIRSPEED;
|
||||
break;
|
||||
|
||||
case CEL_CRSF_TYPE_HEARTBEAT:
|
||||
/* TODO: parse heartbeat payload (2 bytes).
|
||||
* Layout: [0..1] origin_addr (uint16 big-endian) */
|
||||
if (len < 2) return -1;
|
||||
memset(out, 0, sizeof(*out));
|
||||
out->type = CEL_TELEM_HEARTBEAT;
|
||||
break;
|
||||
|
||||
case CEL_CRSF_TYPE_RPM:
|
||||
/* TODO: parse RPM payload (variable length).
|
||||
* Layout:
|
||||
* [0] source_id (uint8)
|
||||
* [1..] rpm values (24-bit signed big-endian, 3 bytes each)
|
||||
* Up to 8 RPM values. */
|
||||
if (len < 4) return -1;
|
||||
memset(out, 0, sizeof(*out));
|
||||
out->type = CEL_TELEM_RPM;
|
||||
break;
|
||||
|
||||
case CEL_CRSF_TYPE_TEMP:
|
||||
/* TODO: parse temperature payload (variable length).
|
||||
* Layout:
|
||||
* [0] source_id (uint8)
|
||||
* [1..] temp values (int16 big-endian, divide by 10.0, deg C)
|
||||
* Up to 8 temperature values. */
|
||||
if (len < 3) return -1;
|
||||
memset(out, 0, sizeof(*out));
|
||||
out->type = CEL_TELEM_TEMP;
|
||||
break;
|
||||
|
||||
case CEL_CRSF_TYPE_VOLTAGES:
|
||||
/* TODO: parse voltage payload (variable length).
|
||||
* Layout:
|
||||
* [0] source_id (uint8)
|
||||
* [1..] cell voltages (uint16 big-endian mV, divide by 1000.0)
|
||||
* Up to 8 cell values. */
|
||||
if (len < 3) return -1;
|
||||
memset(out, 0, sizeof(*out));
|
||||
out->type = CEL_TELEM_VOLTAGES;
|
||||
break;
|
||||
|
||||
case CEL_CRSF_TYPE_ESC_SENSOR:
|
||||
/* TODO: parse ESC sensor payload (12 bytes).
|
||||
* Layout:
|
||||
* [0..1] voltage (uint16 big-endian, divide by 100.0, V)
|
||||
* [2..3] current (uint16 big-endian, divide by 100.0, A)
|
||||
* [4..5] consumed (uint16 big-endian, mAh)
|
||||
* [6..9] rpm (uint32 big-endian)
|
||||
* [10] temp (uint8, deg C) */
|
||||
if (len < 12) return -1;
|
||||
memset(out, 0, sizeof(*out));
|
||||
out->type = CEL_TELEM_ESC;
|
||||
break;
|
||||
|
||||
case CEL_CRSF_TYPE_LINK_STATS:
|
||||
/* TODO: parse link stats payload (10 bytes).
|
||||
* Layout:
|
||||
* [0] uplink_rssi1 (uint8, 0-100%)
|
||||
* [1] uplink_rssi2 (uint8, 0-100%)
|
||||
* [2] uplink_quality (uint8, 0-100%)
|
||||
* [3] uplink_snr (int8, signed dB)
|
||||
* [4] active_antenna (uint8)
|
||||
* [5] rf_mode (uint8)
|
||||
* [6] uplink_power (uint8)
|
||||
* [7] downlink_rssi (uint8, 0-100%)
|
||||
* [8] downlink_qual (uint8, 0-100%)
|
||||
* [9] downlink_snr (int8, signed dB) */
|
||||
if (len < 10) return -1;
|
||||
memset(out, 0, sizeof(*out));
|
||||
out->type = CEL_TELEM_LINK;
|
||||
break;
|
||||
|
||||
case CEL_CRSF_TYPE_ATTITUDE:
|
||||
/* TODO: parse attitude payload (6 bytes).
|
||||
* Layout:
|
||||
* [0..1] pitch (int16 big-endian, divide by 10000.0, radians)
|
||||
* [2..3] roll (int16 big-endian, divide by 10000.0, radians)
|
||||
* [4..5] yaw (int16 big-endian, divide by 10000.0, radians) */
|
||||
if (len < 6) return -1;
|
||||
memset(out, 0, sizeof(*out));
|
||||
out->type = CEL_TELEM_ATTITUDE;
|
||||
break;
|
||||
|
||||
case CEL_CRSF_TYPE_FLIGHT_MODE:
|
||||
/* TODO: parse flight mode payload (variable length string).
|
||||
* Null-terminated ASCII string. Copy into out->data.flight_mode.name
|
||||
* with truncation at 31 chars + null. */
|
||||
if (len == 0) return -1;
|
||||
memset(out, 0, sizeof(*out));
|
||||
out->type = CEL_TELEM_FLIGHT_MODE;
|
||||
break;
|
||||
|
||||
default:
|
||||
return -1;
|
||||
default:
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -38,10 +38,10 @@ typedef struct {
|
||||
|
||||
/* Battery sensor */
|
||||
typedef struct {
|
||||
float voltage_v;
|
||||
float current_a;
|
||||
uint32_t capacity_mah;
|
||||
uint8_t remaining_pct;
|
||||
uint16_t voltage_mv; /* millivolts */
|
||||
uint16_t current_ma; /* milliamps */
|
||||
uint16_t capacity_mah; /* mAh consumed */
|
||||
uint8_t remaining_pct; /* percentage remaining */
|
||||
} cel_telem_battery;
|
||||
|
||||
/* Barometric altitude */
|
||||
@@ -52,7 +52,7 @@ typedef struct {
|
||||
|
||||
/* Airspeed */
|
||||
typedef struct {
|
||||
float speed_kmh;
|
||||
uint16_t speed_kmh; /* km/h * 10 (0.1 km/h resolution) */
|
||||
} cel_telem_airspeed;
|
||||
|
||||
/* Heartbeat */
|
||||
|
||||
Reference in New Issue
Block a user