[crsf_rc] Add ability to inject buffers for development

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,9 +406,39 @@ 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;
 		}
 

src/drivers/rc/crsf_rc/CrsfParser.hpp

@@ -43,6 +43,7 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <board_config.h>
 
 #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);

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", "<file:dev>", "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;

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.