From 34dd25fecbf7ae71ee984600d38eb9f9a4ac14d7 Mon Sep 17 00:00:00 2001 From: portersky <24420859+portersky@users.noreply.github.com> Date: Sun, 14 Jun 2026 21:02:23 +0200 Subject: [PATCH] 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. --- celrs/crsf_telemetry.c | 195 +++++++++--------------------------- celrs/crsf_telemetry.h | 10 +- tests/CMakeLists.txt | 8 ++ tests/test_crsf_telemetry.c | 147 +++++++++++++++++++++++++++ 4 files changed, 206 insertions(+), 154 deletions(-) create mode 100644 tests/test_crsf_telemetry.c diff --git a/celrs/crsf_telemetry.c b/celrs/crsf_telemetry.c index cb3e6c4..8757e15 100644 --- a/celrs/crsf_telemetry.c +++ b/celrs/crsf_telemetry.c @@ -1,164 +1,61 @@ #include "celrs/crsf_telemetry.h" #include +/* 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; } diff --git a/celrs/crsf_telemetry.h b/celrs/crsf_telemetry.h index 237b04f..46c6466 100644 --- a/celrs/crsf_telemetry.h +++ b/celrs/crsf_telemetry.h @@ -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 */ diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 5dd0729..54147cc 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -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}") diff --git a/tests/test_crsf_telemetry.c b/tests/test_crsf_telemetry.c new file mode 100644 index 0000000..29b9c32 --- /dev/null +++ b/tests/test_crsf_telemetry.c @@ -0,0 +1,147 @@ +#include "unity.h" +#include "celrs/crsf.h" +#include + +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(); +}