portersky/celrs
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: implement telemetry parser

Browse Source 
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:
portersky
2026-06-14 21:02:23 +02:00
parent b97a7c5b3a
commit 34dd25fecb
4 changed files with 206 additions and 154 deletions
Download Patch File Download Diff File Expand all files Collapse all files
celrs/crsf_telemetry.c
+46 -149
View File
@@ -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;
}
celrs/crsf_telemetry.h
+5 -5
View File
@@ -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 */
tests/CMakeLists.txt
+8
View File
@@ -46,6 +46,14 @@ target_compile_features(test_crsf_stream PRIVATE c_std_23)
add_test(NAME test_crsf_stream COMMAND test_crsf_stream)
list(APPEND TEST_TARGETS test_crsf_stream)
# CRSF telemetry tests
add_executable(test_crsf_telemetry test_crsf_telemetry.c)
target_include_directories(test_crsf_telemetry PRIVATE "${CMAKE_SOURCE_DIR}")
target_link_libraries(test_crsf_telemetry PRIVATE celrs_crsf Unity::Unity)
target_compile_features(test_crsf_telemetry PRIVATE c_std_23)
add_test(NAME test_crsf_telemetry COMMAND test_crsf_telemetry)
list(APPEND TEST_TARGETS test_crsf_telemetry)
# Serial tests — mocks the platform backend (serial_internal.h)
add_executable(test_serial test_serial.c)
target_include_directories(test_serial PRIVATE "${CMAKE_SOURCE_DIR}")
tests/test_crsf_telemetry.c
+147
View File
@@ -0,0 +1,147 @@
#include "unity.h"
#include "celrs/crsf.h"
#include <string.h>
void setUp(void) {}
void tearDown(void) {}
/* Helper: build a valid test frame with known CRC */
static size_t build_frame(uint8_t* dst, uint8_t addr, uint8_t type,
uint8_t const* payload, uint8_t payload_len) {
uint8_t length = 1 + payload_len + 1;
dst[0] = addr;
dst[1] = length;
dst[2] = type;
memcpy(dst + 3, payload, payload_len);
uint8_t crc = cel_crsf_crc(dst + 2, 1 + payload_len);
dst[2 + length - 1] = crc;
return 2 + length;
}
/* Telemetry parse tests */
void test_parse_link_stats(void) {
uint8_t buf[32];
uint8_t payload[10] = {
80, 70, 95, 0x7F, /* rssi1, rssi2, uplink_qual, snr(-127..127) */
0, 5, 20, 60, 90, 0x60 /* antenna, rf_mode, power, down_rssi, down_qual, snr */
};
build_frame(buf, 0x08, CEL_CRSF_TYPE_LINK_STATS, payload, 10);
cel_crsf_frame frame;
TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, buf, sizeof(buf)));
cel_telemetry telem;
TEST_ASSERT_EQUAL_INT(0, cel_crsf_telemetry_parse(&frame, &telem));
TEST_ASSERT_EQUAL_UINT(CEL_TELEM_LINK, telem.type);
TEST_ASSERT_EQUAL_UINT8(80, telem.data.link.uplink_rssi1);
TEST_ASSERT_EQUAL_UINT8(70, telem.data.link.uplink_rssi2);
TEST_ASSERT_EQUAL_UINT8(95, telem.data.link.uplink_quality);
TEST_ASSERT_EQUAL_INT8(0x7F, telem.data.link.uplink_snr);
TEST_ASSERT_EQUAL_UINT8(0, telem.data.link.active_antenna);
TEST_ASSERT_EQUAL_UINT8(5, telem.data.link.rf_mode);
TEST_ASSERT_EQUAL_UINT8(20, telem.data.link.uplink_power);
TEST_ASSERT_EQUAL_UINT8(60, telem.data.link.downlink_rssi);
TEST_ASSERT_EQUAL_UINT8(90, telem.data.link.downlink_qual);
TEST_ASSERT_EQUAL_INT8(0x60, telem.data.link.downlink_snr);
}
void test_parse_link_stats_short(void) {
uint8_t buf[32];
uint8_t payload[5] = {80, 70, 95, 0x7F, 0};
build_frame(buf, 0x08, CEL_CRSF_TYPE_LINK_STATS, payload, 5);
cel_crsf_frame frame;
TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, buf, sizeof(buf)));
cel_telemetry telem;
TEST_ASSERT_EQUAL_INT(-1, cel_crsf_telemetry_parse(&frame, &telem));
}
void test_parse_battery(void) {
uint8_t buf[32];
uint8_t payload[7] = {
0x40, 0x03, /* voltage: 0x0340 = 832 -> 0.832V */
0x00, 0x00, /* current: 0 */
0x00, 0x00, /* capacity: 0 */
0x64 /* remaining: 100% */
};
build_frame(buf, 0x08, CEL_CRSF_TYPE_BATTERY, payload, 7);
cel_crsf_frame frame;
TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, buf, sizeof(buf)));
cel_telemetry telem;
TEST_ASSERT_EQUAL_INT(0, cel_crsf_telemetry_parse(&frame, &telem));
TEST_ASSERT_EQUAL_UINT(CEL_TELEM_BATTERY, telem.type);
TEST_ASSERT_EQUAL_UINT16(0x0340, telem.data.battery.voltage_mv);
TEST_ASSERT_EQUAL_UINT8(0x64, telem.data.battery.remaining_pct);
}
void test_parse_heartbeat(void) {
uint8_t buf[32];
uint8_t payload[2] = {0x10, 0x80}; /* origin_addr = 0x8010 */
build_frame(buf, 0x10, CEL_CRSF_TYPE_HEARTBEAT, payload, 2);
cel_crsf_frame frame;
TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, buf, sizeof(buf)));
cel_telemetry telem;
TEST_ASSERT_EQUAL_INT(0, cel_crsf_telemetry_parse(&frame, &telem));
TEST_ASSERT_EQUAL_UINT(CEL_TELEM_HEARTBEAT, telem.type);
TEST_ASSERT_EQUAL_UINT16(0x8010, telem.data.heartbeat.origin_addr);
}
void test_parse_airspeed(void) {
uint8_t buf[32];
uint8_t payload[2] = {0x00, 0x01}; /* speed = 0x0100 = 256 km/h */
build_frame(buf, 0x08, CEL_CRSF_TYPE_AIRSPEED, payload, 2);
cel_crsf_frame frame;
TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, buf, sizeof(buf)));
cel_telemetry telem;
TEST_ASSERT_EQUAL_INT(0, cel_crsf_telemetry_parse(&frame, &telem));
TEST_ASSERT_EQUAL_UINT(CEL_TELEM_AIRSPEED, telem.type);
TEST_ASSERT_EQUAL_UINT16(256, telem.data.airspeed.speed_kmh);
}
void test_parse_unknown_type(void) {
uint8_t buf[32];
uint8_t payload[2] = {0x00, 0x01};
build_frame(buf, 0x08, 0xFF, payload, 2);
cel_crsf_frame frame;
TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, buf, sizeof(buf)));
cel_telemetry telem;
TEST_ASSERT_EQUAL_INT(-1, cel_crsf_telemetry_parse(&frame, &telem));
}
void test_parse_null_args(void) {
cel_telemetry telem;
TEST_ASSERT_EQUAL_INT(-1, cel_crsf_telemetry_parse(NULL, &telem));
}
void test_parse_null_out(void) {
uint8_t buf[32];
uint8_t payload[2] = {0x10, 0x80};
build_frame(buf, 0x10, CEL_CRSF_TYPE_HEARTBEAT, payload, 2);
cel_crsf_frame frame;
TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, buf, sizeof(buf)));
TEST_ASSERT_EQUAL_INT(-1, cel_crsf_telemetry_parse(&frame, NULL));
}
int main(void) {
UNITY_BEGIN();
RUN_TEST(test_parse_link_stats);
RUN_TEST(test_parse_link_stats_short);
RUN_TEST(test_parse_battery);
RUN_TEST(test_parse_heartbeat);
RUN_TEST(test_parse_airspeed);
RUN_TEST(test_parse_unknown_type);
RUN_TEST(test_parse_null_args);
RUN_TEST(test_parse_null_out);
return UNITY_END();
}