diff --git a/celrs/crsf.c b/celrs/crsf.c index 87dd5bc..51ce4e3 100644 --- a/celrs/crsf.c +++ b/celrs/crsf.c @@ -133,52 +133,7 @@ void cel_crsf_channel_default(int16_t channels[16]) { } /* --------------------------------------------------------------------------- */ -/* Deprecated (old frame format) */ +/* Telemetry parsing */ /* --------------------------------------------------------------------------- */ -int cel_crsf_frame_validate(cel_crsf_frame_legacy const* frame) { - uint8_t data[260]; - size_t offset = 0; - data[offset++] = frame->destination; - data[offset++] = frame->source; - data[offset++] = frame->type; - data[offset++] = frame->size; - memcpy(data + offset, frame->payload, frame->size); - offset += frame->size; - uint8_t calc_crc = cel_crsf_crc(data, offset); - return calc_crc == frame->crc ? 0 : -1; -} - -int cel_crsf_frame_parse_legacy(cel_crsf_frame_legacy* frame, uint8_t const* buf, - size_t len) { - if (frame == NULL || buf == NULL) return -1; - if (len < 5) return -1; - if (buf[0] != CEL_CRSF_FRAME_HEADER) return -1; - frame->destination = buf[1]; - frame->source = buf[2]; - frame->type = buf[3]; - frame->size = buf[4]; - uint8_t size = buf[4]; - size_t total = 6 + size; - if (len < total) return -1; - memcpy(frame->payload, buf + 5, size); - frame->crc = buf[5 + size]; - return cel_crsf_frame_validate(frame); -} - -size_t cel_crsf_frame_build_legacy(uint8_t* dst, uint8_t destination, - uint8_t source, uint8_t type, - uint8_t const* payload, uint8_t size) { - if (dst == NULL) return 0; - dst[0] = CEL_CRSF_FRAME_HEADER; - dst[1] = destination; - dst[2] = source; - dst[3] = type; - dst[4] = size; - if (payload != NULL && size > 0) { - memcpy(dst + 5, payload, size); - } - uint8_t crc = cel_crsf_crc(dst + 1, 3 + 1 + size); - dst[5 + size] = crc; - return 1 + 3 + 1 + size + 1; -} \ No newline at end of file +/* See crsf_telemetry.c */ \ No newline at end of file diff --git a/celrs/crsf.h b/celrs/crsf.h index e5a9574..6552279 100644 --- a/celrs/crsf.h +++ b/celrs/crsf.h @@ -109,22 +109,3 @@ void cel_crsf_channel_default(int16_t channels[16]); /* Parameter protocol — see crsf_param.h */ #include "celrs/crsf_param.h" - -/* --- Deprecated (old frame format: [0xC8][dest][src][type][size][payload][crc]) --- */ - -/* Deprecated frame structure (old format). Use cel_crsf_frame instead. */ -typedef struct { - uint8_t destination; - uint8_t source; - uint8_t type; - uint8_t size; - uint8_t payload[255]; - uint8_t crc; -} cel_crsf_frame_legacy; - -int cel_crsf_frame_validate(cel_crsf_frame_legacy const* frame); -int cel_crsf_frame_parse_legacy(cel_crsf_frame_legacy* frame, uint8_t const* buf, - size_t len); -size_t cel_crsf_frame_build_legacy(uint8_t* dst, uint8_t destination, - uint8_t source, uint8_t type, - uint8_t const* payload, uint8_t size); diff --git a/tests/test_crsf.c b/tests/test_crsf.c index 308b2ff..330c872 100644 --- a/tests/test_crsf.c +++ b/tests/test_crsf.c @@ -26,80 +26,17 @@ void test_crc_known_value(void) { TEST_ASSERT_EQUAL_UINT8(crc, crc2); } -/* TODO: rewrite parse/build tests for new ELRS frame format. - * New format: [addr][length][type][payload...][crc] - * Old tests below use legacy functions for backward compat. */ - -/* Frame parse tests (legacy format) */ -void test_parse_legacy_invalid_header(void) { - cel_crsf_frame_legacy frame; - uint8_t buf[8] = {0x00, 0x10, 0x80, 0x03, 0x02, 0x80, 0x01, 0x00}; - TEST_ASSERT_EQUAL_INT(-1, cel_crsf_frame_parse_legacy(&frame, buf, 8)); +/* Channel helper tests */ +void test_channel_clamp_min(void) { + TEST_ASSERT_EQUAL_INT16(CEL_CRSF_CH_MIN, cel_crsf_channel_clamp(0)); } -void test_parse_legacy_too_short(void) { - cel_crsf_frame_legacy frame; - uint8_t buf[2] = {0xC8, 0x10}; - TEST_ASSERT_EQUAL_INT(-1, cel_crsf_frame_parse_legacy(&frame, buf, 2)); +void test_channel_clamp_max(void) { + TEST_ASSERT_EQUAL_INT16(CEL_CRSF_CH_MAX, cel_crsf_channel_clamp(2048)); } -void test_parse_legacy_null_frame(void) { - uint8_t buf[8] = {0xC8, 0x10, 0x80, 0x03, 0x02, 0x80, 0x01, 0x00}; - TEST_ASSERT_EQUAL_INT(-1, cel_crsf_frame_parse_legacy(NULL, buf, 8)); -} - -void test_parse_legacy_null_buf(void) { - cel_crsf_frame_legacy frame; - TEST_ASSERT_EQUAL_INT(-1, cel_crsf_frame_parse_legacy(&frame, NULL, 8)); -} - -/* Frame build tests (legacy format) */ -void test_build_legacy_heartbeat(void) { - uint8_t dst[256]; - uint8_t payload[2] = {0x80, 0x01}; - size_t len = cel_crsf_frame_build_legacy(dst, 0x00, 0x80, 0x03, - payload, 2); - - TEST_ASSERT_GREATER_THAN(0, len); - TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_FRAME_HEADER, dst[0]); - TEST_ASSERT_EQUAL_UINT8(0x00, dst[1]); /* destination */ - TEST_ASSERT_EQUAL_UINT8(0x80, dst[2]); /* source */ - TEST_ASSERT_EQUAL_UINT8(0x03, dst[3]); /* type: heartbeat */ - TEST_ASSERT_EQUAL_UINT8(0x02, dst[4]); /* size */ - TEST_ASSERT_EQUAL_UINT8(0x80, dst[5]); /* payload[0] */ - TEST_ASSERT_EQUAL_UINT8(0x01, dst[6]); /* payload[1] */ -} - -void test_build_legacy_roundtrip(void) { - uint8_t dst[256]; - uint8_t payload[4] = {0xAA, 0xBB, 0xCC, 0xDD}; - size_t len = cel_crsf_frame_build_legacy(dst, 0x10, 0x80, 0x01, - payload, 4); - - /* Parse the built frame back */ - cel_crsf_frame_legacy frame; - TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse_legacy(&frame, dst, len)); - TEST_ASSERT_EQUAL_UINT8(0x10, frame.destination); - TEST_ASSERT_EQUAL_UINT8(0x80, frame.source); - TEST_ASSERT_EQUAL_UINT8(0x01, frame.type); - TEST_ASSERT_EQUAL_UINT8(4, frame.size); - TEST_ASSERT_EQUAL_UINT8(0xAA, frame.payload[0]); - TEST_ASSERT_EQUAL_UINT8(0xDD, frame.payload[3]); -} - -void test_build_legacy_null_dst(void) { - uint8_t payload[2] = {0x01, 0x02}; - TEST_ASSERT_EQUAL_UINT(0, cel_crsf_frame_build_legacy(NULL, 0x00, 0x80, - 0x03, payload, 2)); -} - -void test_build_legacy_null_payload(void) { - uint8_t dst[256]; - size_t len = cel_crsf_frame_build_legacy(dst, 0x10, 0x80, 0x03, NULL, 0); - TEST_ASSERT_GREATER_THAN(0, len); - /* Should still have valid CRC for empty payload */ - cel_crsf_frame_legacy frame; - TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse_legacy(&frame, dst, len)); +void test_channel_clamp_mid(void) { + TEST_ASSERT_EQUAL_INT16(CEL_CRSF_CH_MID, cel_crsf_channel_clamp(CEL_CRSF_CH_MID)); } int main(void) { @@ -107,13 +44,8 @@ int main(void) { RUN_TEST(test_crc_empty); RUN_TEST(test_crc_single_byte); RUN_TEST(test_crc_known_value); - RUN_TEST(test_parse_legacy_invalid_header); - RUN_TEST(test_parse_legacy_too_short); - RUN_TEST(test_parse_legacy_null_frame); - RUN_TEST(test_parse_legacy_null_buf); - RUN_TEST(test_build_legacy_heartbeat); - RUN_TEST(test_build_legacy_roundtrip); - RUN_TEST(test_build_legacy_null_dst); - RUN_TEST(test_build_legacy_null_payload); + RUN_TEST(test_channel_clamp_min); + RUN_TEST(test_channel_clamp_max); + RUN_TEST(test_channel_clamp_mid); return UNITY_END(); } diff --git a/tools/telemetry.c b/tools/telemetry.c index a3b2df8..83494c2 100644 --- a/tools/telemetry.c +++ b/tools/telemetry.c @@ -11,32 +11,6 @@ #include #endif -/* 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 sleep_ms(int ms) { #ifdef _WIN32 Sleep((DWORD)ms); @@ -106,58 +80,12 @@ int main(int argc, char const* argv[]) { snprintf(msg, sizeof(msg), "Connected to %s (%d baud)", port_path, baud_rate); cel_log_info(msg); - /* TODO: update for new ELRS frame format. - * New format uses cel_crsf_build_rc_frame() and cel_crsf_stream_feed(). */ - - /* Send heartbeat to establish CRSF link (legacy format) */ - 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_legacy(hb_frame, - CEL_CRSF_ADDRESS_FC_BROADCAST, - CEL_CRSF_ADDRESS_TBS_GROUND_STATION, - 0x03, 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)); - if (n == 0) { - sleep_ms(interval_ms); - continue; - } - - cel_crsf_frame_legacy frame; - if (cel_crsf_frame_parse_legacy(&frame, buf, n) != 0) { - errors++; - continue; - } - - if (frame.type != 0x02) { - 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); + /* TODO: implement telemetry read loop. + * 1. Use cel_crsf_stream_create() for incremental parsing. + * 2. Read raw bytes with cel_serial_read(). + * 3. Feed to cel_crsf_stream_feed() to extract frames. + * 4. Parse telemetry with cel_crsf_telemetry_parse(). + * 5. Print link stats, battery, GPS, etc. */ cel_serial_close(port); return 0;