From bb72088ff6cdc3b592ca642006d34dc1c7ad20e5 Mon Sep 17 00:00:00 2001 From: Niklas Hauser Date: Wed, 17 Sep 2025 00:38:06 +0800 Subject: [PATCH] [crsf_rc] Add ability to inject buffers for development --- src/drivers/rc/crsf_rc/CrsfParser.cpp | 45 +++++++++++++++++++++++++++ src/drivers/rc/crsf_rc/CrsfParser.hpp | 4 +++ src/drivers/rc/crsf_rc/CrsfRc.cpp | 40 ++++++++++++++++++++++++ src/drivers/rc/crsf_rc/Kconfig | 6 ++++ 4 files changed, 95 insertions(+) diff --git a/src/drivers/rc/crsf_rc/CrsfParser.cpp b/src/drivers/rc/crsf_rc/CrsfParser.cpp index 61d2029b0d..b8a1f6bf77 100644 --- a/src/drivers/rc/crsf_rc/CrsfParser.cpp +++ b/src/drivers/rc/crsf_rc/CrsfParser.cpp @@ -143,6 +143,11 @@ static uint32_t working_segment_size = HEADER_SIZE; #define RX_QUEUE_BUFFER_SIZE 200 static QueueBuffer_t rx_queue; static uint8_t rx_queue_buffer[RX_QUEUE_BUFFER_SIZE]; +#ifdef CONFIG_DRIVERS_RC_CRSF_RC_INJECT +static QueueBuffer_t inject_queue; +static uint8_t inject_queue_buffer[RX_QUEUE_BUFFER_SIZE]; +static uint8_t temp_queue_buffer[RX_QUEUE_BUFFER_SIZE]; +#endif static uint8_t process_buffer[CRSF_MAX_PACKET_LEN]; static CrsfPacketDescriptor_t *working_descriptor = NULL; @@ -151,6 +156,9 @@ static CrsfPacketDescriptor_t *FindCrsfDescriptor(const enum CRSF_PACKET_TYPE pa void CrsfParser_Init(void) { QueueBuffer_Init(&rx_queue, rx_queue_buffer, RX_QUEUE_BUFFER_SIZE); +#ifdef CONFIG_DRIVERS_RC_CRSF_RC_INJECT + QueueBuffer_Init(&inject_queue, inject_queue_buffer, RX_QUEUE_BUFFER_SIZE); +#endif } static float ConstrainF(const float x, const float min, const float max) @@ -269,6 +277,13 @@ bool CrsfParser_LoadBuffer(const uint8_t *buffer, const uint32_t size) return QueueBuffer_AppendBuffer(&rx_queue, buffer, size); } +#ifdef CONFIG_DRIVERS_RC_CRSF_RC_INJECT +bool CrsfParser_InjectBuffer(const uint8_t *buffer, const uint32_t size) +{ + return QueueBuffer_AppendBuffer(&inject_queue, buffer, size); +} +#endif + uint32_t CrsfParser_FreeQueueSize(void) { return RX_QUEUE_BUFFER_SIZE - QueueBuffer_Count(&rx_queue); @@ -391,7 +406,37 @@ bool CrsfParser_TryParseCrsfPacket(CrsfPacket_t *const new_packet, CrsfParserSta parser_state = PARSER_STATE_HEADER; if (valid_packet) { +#ifdef CONFIG_DRIVERS_RC_CRSF_RC_INJECT + + if (!QueueBuffer_IsEmpty(&inject_queue)) { + // copy the remaining bytes from the rx queue to the temp buffer + const uint32_t temp_size = QueueBuffer_Count(&rx_queue); + + if (temp_size) { + QueueBuffer_PeekBuffer(&rx_queue, 0, temp_queue_buffer, temp_size); + // clear the rx queue + QueueBuffer_Dequeue(&rx_queue, QueueBuffer_Count(&rx_queue)); + } + + // append the inject queue to the rx queue + uint8_t inject_byte; + + while (QueueBuffer_Get(&inject_queue, &inject_byte)) { + QueueBuffer_Append(&rx_queue, inject_byte); + } + + if (temp_size) { + // append the temp buffer back to the rx queue + QueueBuffer_AppendBuffer(&rx_queue, temp_queue_buffer, temp_size); + } + + } else { + return true; + } + +#else return true; +#endif } break; diff --git a/src/drivers/rc/crsf_rc/CrsfParser.hpp b/src/drivers/rc/crsf_rc/CrsfParser.hpp index 167cd6466e..d0f5bab46d 100644 --- a/src/drivers/rc/crsf_rc/CrsfParser.hpp +++ b/src/drivers/rc/crsf_rc/CrsfParser.hpp @@ -43,6 +43,7 @@ #include #include +#include #define CRSF_CHANNEL_COUNT 16 @@ -108,5 +109,8 @@ typedef struct { void CrsfParser_Init(void); bool CrsfParser_LoadBuffer(const uint8_t *buffer, const uint32_t size); +#ifdef DRIVERS_RC_CRSF_RC_INJECT +bool CrsfParser_InjectBuffer(const uint8_t *buffer, const uint32_t size); +#endif uint32_t CrsfParser_FreeQueueSize(void); bool CrsfParser_TryParseCrsfPacket(CrsfPacket_t *const new_packet, CrsfParserStatistics_t *const parser_statistics); diff --git a/src/drivers/rc/crsf_rc/CrsfRc.cpp b/src/drivers/rc/crsf_rc/CrsfRc.cpp index 09122a3b33..6535fceb1d 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.cpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.cpp @@ -549,6 +549,43 @@ int CrsfRc::print_status() int CrsfRc::custom_command(int argc, char *argv[]) { +#ifdef CONFIG_DRIVERS_RC_CRSF_RC_INJECT + + if (!strcmp(argv[0], "start")) { + if (is_running()) { + return print_usage("already running"); + } + + int ret = CrsfRc::task_spawn(argc, argv); + + if (ret) { + return ret; + } + } + + if (!is_running()) { + return print_usage("not running"); + } + + // crsf_rc inject 0x7C 0xC8 0xEA 0x30 0x02 0x59 0x31 0x00 0x6A + if (!strcmp(argv[0], "inject")) { + const uint8_t length = argc; + uint8_t buf[100]; + buf[0] = 0xC8; // sync byte + buf[1] = length; + uint8_t i = 0; + + for (; i < length - 1; i++) { + buf[i + 2] = (uint8_t) strtol(argv[i + 1], nullptr, 16); + } + + buf[i + 2] = Crc8Calc(buf + 2, length - 1); // CRC + CrsfParser_InjectBuffer(buf, length + 2); + return PX4_OK; + } + +#endif + return print_usage("unknown command"); } @@ -570,6 +607,9 @@ This module parses the CRSF RC uplink protocol and generates CRSF downlink telem PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "", "RC device", true); PRINT_MODULE_USAGE_PARAM_INT('b', 420000, 4800, 3000000, "RC baudrate", true); +#ifdef CONFIG_DRIVERS_RC_CRSF_RC_INJECT + PRINT_MODULE_USAGE_COMMAND_DESCR("inject", "Inject frame data bytes (for testing)"); +#endif PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/drivers/rc/crsf_rc/Kconfig b/src/drivers/rc/crsf_rc/Kconfig index 21096275af..2e74abf782 100644 --- a/src/drivers/rc/crsf_rc/Kconfig +++ b/src/drivers/rc/crsf_rc/Kconfig @@ -3,3 +3,9 @@ menuconfig DRIVERS_RC_CRSF_RC default n ---help--- Enable support for crsf rc + +config DRIVERS_RC_CRSF_RC_INJECT + bool "Inject CRSF RC" + default n + ---help--- + Enable this to inject CRSF RC commands.