drivers/rc/crsf_rc: update standalone driver for CRSF and direct support for lq/rssi (#20084)

Co-authored-by: Chris Seto <chris1seto@gmail.com>

boards/bitcraze/crazyflie/syslink/syslink_main.cpp

@@ -548,6 +548,9 @@ Syslink::handle_raw(syslink_message_t *sys)
 		rc.values[3] = cmd->thrust * 1000 / USHRT_MAX + 1000;
 		rc.values[4] = 1000; // Dummy channel as px4 needs at least 5
 
+		rc.link_quality = -1;
+		rc.rssi_dbm = NAN;
+
 		_rc_pub.publish(rc);
 
 	} else if (c->port == CRTP_PORT_MAVLINK) {

boards/emlid/navio2/navio_sysfs_rc_in/NavioSysRCInput.cpp

@@ -152,6 +152,9 @@ void NavioSysRCInput::Run()
 	data.timestamp_last_signal = timestamp_sample;
 	data.channel_count = CHANNELS;
 	data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
+	data.link_quality = -1;
+	data.rssi_dbm = NAN;
+
 	data.timestamp = hrt_absolute_time();
 
 	_input_rc_pub.publish(data);

msg/input_rc.msg

@@ -35,3 +35,6 @@ uint16 rc_ppm_frame_length		# Length of a single PPM frame. Zero for non-PPM sys
 
 uint8 input_source			# Input source
 uint16[18] values			# measured pulse widths for each of the supported channels
+
+int8 link_quality			# link quality. Percentage 0-100%. -1 = invalid
+float32 rssi_dbm			# Actual rssi in units of dBm. NaN = invalid

src/drivers/px4io/px4io.cpp

@@ -1144,6 +1144,9 @@ int PX4IO::io_publish_raw_rc()
 
 	if (input_rc.input_source != input_rc_s::RC_INPUT_SOURCE_UNKNOWN) {
 
+		input_rc.link_quality = -1;
+		input_rc.rssi_dbm = NAN;
+
 		_to_input_rc.publish(input_rc);
 	}
 

src/drivers/rc/crsf_rc/CMakeLists.txt

@@ -35,10 +35,14 @@ px4_add_module(
 	MAIN crsf_rc
 	COMPILE_FLAGS
 	SRCS
-		crsf.cpp
-		crsf.h
 		CrsfRc.cpp
 		CrsfRc.hpp
+		QueueBuffer.cpp
+		QueueBuffer.hpp
+		CrsfParser.cpp
+		CrsfParser.hpp
+		Crc8.hpp
+		Crc8.cpp
 
 	MODULE_CONFIG
 		module.yaml

src/drivers/rc/crsf_rc/Crc8.cpp

@@ -0,0 +1,29 @@
+#include <stdbool.h>
+#include <stdint.h>
+#include "Crc8.hpp"
+
+static uint8_t crc8_lut[256];
+
+void Crc8Init(const uint8_t poly)
+{
+	for (int idx = 0; idx < 256; ++idx) {
+		uint8_t crc = idx;
+
+		for (int shift = 0; shift < 8; ++shift) {
+			crc = (crc << 1) ^ ((crc & 0x80) ? poly : 0);
+		}
+
+		crc8_lut[idx] = crc & 0xff;
+	}
+}
+
+uint8_t Crc8Calc(const uint8_t *data, uint8_t size)
+{
+	uint8_t crc = 0;
+
+	while (size--) {
+		crc = crc8_lut[crc ^ *data++];
+	}
+
+	return crc;
+}

src/drivers/rc/crsf_rc/Crc8.hpp

@@ -0,0 +1,7 @@
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+void Crc8Init(const uint8_t poly);
+uint8_t Crc8Calc(const uint8_t *data, uint8_t size);

src/drivers/rc/crsf_rc/CrsfParser.cpp

@@ -0,0 +1,352 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+* @file CrsfParser.cpp
+*
+* Parser for incoming CRSF packets
+*
+* @author Chris Seto <chris1seto@gmail.com>
+*/
+
+#include <stdio.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+#include "QueueBuffer.hpp"
+#include "CrsfParser.hpp"
+#include "Crc8.hpp"
+
+#define CRSF_CHANNEL_VALUE_MIN  172
+#define CRSF_CHANNEL_VALUE_MAX  1811
+#define CRSF_CHANNEL_VALUE_SPAN (CRSF_CHANNEL_VALUE_MAX - CRSF_CHANNEL_VALUE_MIN)
+#define CRSF_MAX_PACKET_LEN 64
+#define CRSF_HEADER 0xc8
+
+enum CRSF_PAYLOAD_SIZE {
+	CRSF_PAYLOAD_SIZE_GPS = 15,
+	CRSF_PAYLOAD_SIZE_BATTERY = 8,
+	CRSF_PAYLOAD_SIZE_LINK_STATISTICS = 10,
+	CRSF_PAYLOAD_SIZE_RC_CHANNELS = 22,
+	CRSF_PAYLOAD_SIZE_ATTITUDE = 6,
+};
+
+enum CRSF_PACKET_TYPE {
+	CRSF_PACKET_TYPE_GPS = 0x02,
+	CRSF_PACKET_TYPE_BATTERY_SENSOR = 0x08,
+	CRSF_PACKET_TYPE_LINK_STATISTICS = 0x14,
+	CRSF_PACKET_TYPE_OPENTX_SYNC = 0x10,
+	CRSF_PACKET_TYPE_RADIO_ID = 0x3A,
+	CRSF_PACKET_TYPE_RC_CHANNELS_PACKED = 0x16,
+	CRSF_PACKET_TYPE_ATTITUDE = 0x1E,
+	CRSF_PACKET_TYPE_FLIGHT_MODE = 0x21,
+	// Extended Header Frames, range: 0x28 to 0x96
+	CRSF_PACKET_TYPE_DEVICE_PING = 0x28,
+	CRSF_PACKET_TYPE_DEVICE_INFO = 0x29,
+	CRSF_PACKET_TYPE_PARAMETER_SETTINGS_ENTRY = 0x2B,
+	CRSF_PACKET_TYPE_PARAMETER_READ = 0x2C,
+	CRSF_PACKET_TYPE_PARAMETER_WRITE = 0x2D,
+	CRSF_PACKET_TYPE_COMMAND = 0x32,
+	// MSP commands
+	CRSF_PACKET_TYPE_MSP_REQ = 0x7A,   // response request using msp sequence as command
+	CRSF_PACKET_TYPE_MSP_RESP = 0x7B,  // reply with 58 byte chunked binary
+	CRSF_PACKET_TYPE_MSP_WRITE = 0x7C, // write with 8 byte chunked binary (OpenTX outbound telemetry buffer limit)
+};
+
+enum CRSF_ADDRESS {
+	CRSF_ADDRESS_BROADCAST = 0x00,
+	CRSF_ADDRESS_USB = 0x10,
+	CRSF_ADDRESS_TBS_CORE_PNP_PRO = 0x80,
+	CRSF_ADDRESS_RESERVED1 = 0x8A,
+	CRSF_ADDRESS_CURRENT_SENSOR = 0xC0,
+	CRSF_ADDRESS_GPS = 0xC2,
+	CRSF_ADDRESS_TBS_BLACKBOX = 0xC4,
+	CRSF_ADDRESS_FLIGHT_CONTROLLER = 0xC8,
+	CRSF_ADDRESS_RESERVED2 = 0xCA,
+	CRSF_ADDRESS_RACE_TAG = 0xCC,
+	CRSF_ADDRESS_RADIO_TRANSMITTER = 0xEA,
+	CRSF_ADDRESS_CRSF_RECEIVER = 0xEC,
+	CRSF_ADDRESS_CRSF_TRANSMITTER = 0xEE,
+};
+
+#define HEADER_SIZE           1
+#define PACKET_SIZE_SIZE      1
+#define PACKET_TYPE_SIZE      1
+#define PACKET_SIZE_TYPE_SIZE 2
+#define CRC_SIZE              1
+
+enum PARSER_STATE {
+	PARSER_STATE_HEADER,
+	PARSER_STATE_SIZE_TYPE,
+	PARSER_STATE_PAYLOAD,
+	PARSER_STATE_CRC,
+};
+
+typedef struct {
+	uint8_t packet_type;
+	uint32_t packet_size;
+	bool (*processor)(const uint8_t *data, const uint32_t size, CrsfPacket_t *const new_packet);
+} CrsfPacketDescriptor_t;
+
+static bool ProcessChannelData(const uint8_t *data, const uint32_t size, CrsfPacket_t *const new_packet);
+static bool ProcessLinkStatistics(const uint8_t *data, const uint32_t size, CrsfPacket_t *const new_packet);
+
+#define CRSF_PACKET_DESCRIPTOR_COUNT  2
+static const CrsfPacketDescriptor_t crsf_packet_descriptors[CRSF_PACKET_DESCRIPTOR_COUNT] = {
+	{CRSF_PACKET_TYPE_RC_CHANNELS_PACKED, CRSF_PAYLOAD_SIZE_RC_CHANNELS, ProcessChannelData},
+	{CRSF_PACKET_TYPE_LINK_STATISTICS, CRSF_PAYLOAD_SIZE_LINK_STATISTICS, ProcessLinkStatistics},
+};
+
+static enum PARSER_STATE parser_state = PARSER_STATE_HEADER;
+static uint32_t working_index = 0;
+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];
+static uint8_t process_buffer[CRSF_MAX_PACKET_LEN];
+static CrsfPacketDescriptor_t *working_descriptor = NULL;
+
+static CrsfPacketDescriptor_t *FindCrsfDescriptor(const enum CRSF_PACKET_TYPE packet_type);
+
+void CrsfParser_Init(void)
+{
+	QueueBuffer_Init(&rx_queue, rx_queue_buffer, RX_QUEUE_BUFFER_SIZE);
+}
+
+static float ConstrainF(const float x, const float min, const float max)
+{
+	if (x < min) {
+		return min;
+
+	} else if (x > max) {
+		return max;
+	}
+
+	return x;
+}
+
+static float MapF(const float x, const float in_min, const float in_max, const float out_min, const float out_max)
+{
+	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+static bool ProcessChannelData(const uint8_t *data, const uint32_t size, CrsfPacket_t *const new_packet)
+{
+	uint32_t raw_channels[CRSF_CHANNEL_COUNT];
+	uint32_t i;
+
+	new_packet->message_type = CRSF_MESSAGE_TYPE_RC_CHANNELS;
+
+	// Decode channel data
+	raw_channels[0] = (data[0] | data[1] << 8) & 0x07FF;
+	raw_channels[1] = (data[1]  >> 3 | data[2] << 5) & 0x07FF;
+	raw_channels[2] = (data[2] >> 6 | data[3] << 2 | data[4] << 10) & 0x07FF;
+	raw_channels[3] = (data[4] >> 1 | data[5] << 7) & 0x07FF;
+	raw_channels[4] = (data[5] >> 4 | data[6] << 4) & 0x07FF;
+	raw_channels[5] = (data[6] >> 7 | data[7] << 1 | data[8] << 9) & 0x07FF;
+	raw_channels[6] = (data[8] >> 2 | data[9] << 6) & 0x07FF;
+	raw_channels[7] = (data[9] >> 5 | data[10] << 3) & 0x07FF;
+	raw_channels[8] = (data[11] | data[12] << 8) & 0x07FF;
+	raw_channels[9] = (data[12] >> 3 | data[13] << 5) & 0x07FF;
+	raw_channels[10] = (data[13] >> 6 | data[14] << 2 | data[15] << 10) & 0x07FF;
+	raw_channels[11] = (data[15] >> 1 | data[16] << 7) & 0x07FF;
+	raw_channels[12] = (data[16] >> 4 | data[17] << 4) & 0x07FF;
+	raw_channels[13] = (data[17] >> 7 | data[18] << 1 | data[19] << 9) & 0x07FF;
+	raw_channels[14] = (data[19] >> 2 | data[20] << 6) & 0x07FF;
+	raw_channels[15] = (data[20] >> 5 | data[21] << 3) & 0x07FF;
+
+	for (i = 0; i < CRSF_CHANNEL_COUNT; i++) {
+		raw_channels[i] = ConstrainF(raw_channels[i], CRSF_CHANNEL_VALUE_MIN, CRSF_CHANNEL_VALUE_MAX);
+		new_packet->channel_data.channels[i] = MapF((float)raw_channels[i], CRSF_CHANNEL_VALUE_MIN, CRSF_CHANNEL_VALUE_MAX,
+						       1000.0f, 2000.0f);
+	}
+
+	return true;
+}
+
+static bool ProcessLinkStatistics(const uint8_t *data, const uint32_t size, CrsfPacket_t *const new_packet)
+{
+	new_packet->message_type = CRSF_MESSAGE_TYPE_LINK_STATISTICS;
+
+	new_packet->link_statistics.uplink_rssi_1 = data[0];
+	new_packet->link_statistics.uplink_rssi_2  = data[1];
+	new_packet->link_statistics.uplink_link_quality = data[2];
+	new_packet->link_statistics.uplink_snr = data[3];
+	new_packet->link_statistics.active_antenna = data[4];
+	new_packet->link_statistics.rf_mode = data[5];
+	new_packet->link_statistics.uplink_tx_power = data[6];
+	new_packet->link_statistics.downlink_rssi = data[7];
+	new_packet->link_statistics.downlink_link_quality = data[8];
+	new_packet->link_statistics.downlink_snr = data[9];
+
+	return true;
+}
+
+static CrsfPacketDescriptor_t *FindCrsfDescriptor(const enum CRSF_PACKET_TYPE packet_type)
+{
+	uint32_t i;
+
+	for (i = 0; i < CRSF_PACKET_DESCRIPTOR_COUNT; i++) {
+		if (crsf_packet_descriptors[i].packet_type == packet_type) {
+			return (CrsfPacketDescriptor_t *)&crsf_packet_descriptors[i];
+		}
+	}
+
+	return NULL;
+}
+
+bool CrsfParser_LoadBuffer(const uint8_t *buffer, const uint32_t size)
+{
+	return QueueBuffer_AppendBuffer(&rx_queue, buffer, size);
+}
+
+uint32_t CrsfParser_FreeQueueSize(void)
+{
+	return RX_QUEUE_BUFFER_SIZE - QueueBuffer_Count(&rx_queue);
+}
+
+// 0xC8 [packet len] [packet type] [data] [crc]
+bool CrsfParser_TryParseCrsfPacket(CrsfPacket_t *const new_packet, CrsfParserStatistics_t *const parser_statistics)
+{
+	uint32_t buffer_count;
+	uint8_t working_byte;
+	uint8_t packet_size;
+	uint8_t packet_type;
+	bool valid_packet = false;
+
+	buffer_count = QueueBuffer_Count(&rx_queue);
+
+	// Iterate through the buffer to parse the message out
+	while ((working_index < buffer_count) && (buffer_count - working_index) >= working_segment_size) {
+		switch (parser_state) {
+		// Header
+		case PARSER_STATE_HEADER:
+			if (QueueBuffer_Get(&rx_queue, &working_byte)) {
+				if (working_byte == CRSF_HEADER) {
+					parser_state = PARSER_STATE_SIZE_TYPE;
+					working_segment_size = PACKET_SIZE_TYPE_SIZE;
+					working_index = 0;
+					buffer_count = QueueBuffer_Count(&rx_queue);
+					continue;
+
+				} else {
+					parser_statistics->disposed_bytes++;
+				}
+			}
+
+			working_index = 0;
+			working_segment_size = HEADER_SIZE;
+			break;
+
+		// Packet size type
+		case PARSER_STATE_SIZE_TYPE:
+			QueueBuffer_Peek(&rx_queue, working_index++, &packet_size);
+			QueueBuffer_Peek(&rx_queue, working_index++, &packet_type);
+
+			working_descriptor = FindCrsfDescriptor((enum CRSF_PACKET_TYPE)packet_type);
+
+			// If we know what this packet is...
+			if (working_descriptor != NULL) {
+				// Validate length
+				if (packet_size != working_descriptor->packet_size + PACKET_SIZE_TYPE_SIZE) {
+					parser_statistics->invalid_known_packet_sizes++;
+					parser_state = PARSER_STATE_HEADER;
+					working_segment_size = HEADER_SIZE;
+					working_index = 0;
+					buffer_count = QueueBuffer_Count(&rx_queue);
+					continue;
+				}
+
+				working_segment_size = working_descriptor->packet_size;
+
+			} else {
+				// We don't know what this packet is, so we'll let the parser continue
+				// just so that we can dequeue it in one shot
+				working_segment_size = packet_size + PACKET_SIZE_TYPE_SIZE;
+			}
+
+			parser_state = PARSER_STATE_PAYLOAD;
+			break;
+
+		// Full packet content
+		case PARSER_STATE_PAYLOAD:
+			working_index += working_segment_size;
+			working_segment_size = CRC_SIZE;
+			parser_state = PARSER_STATE_CRC;
+			break;
+
+		// CRC
+		case PARSER_STATE_CRC:
+			// Fetch the suspected packet as a contingous block of memory
+			QueueBuffer_PeekBuffer(&rx_queue, 0, process_buffer, working_index + CRC_SIZE);
+
+			// Verify checksum
+			if (Crc8Calc(process_buffer + PACKET_SIZE_SIZE, working_index - PACKET_SIZE_SIZE) == process_buffer[working_index]) {
+				if (working_descriptor != NULL) {
+					if (working_descriptor->processor != NULL) {
+						if (working_descriptor->processor(process_buffer + PACKET_SIZE_TYPE_SIZE, working_index - PACKET_SIZE_TYPE_SIZE,
+										  new_packet)) {
+							parser_statistics->crcs_valid_known_packets++;
+							valid_packet = true;
+						}
+					}
+
+				} else {
+					// No working_descriptor at this point means unknown packet
+					parser_statistics->crcs_valid_unknown_packets++;
+				}
+
+				// Remove the sucessfully processed data from the queue
+				QueueBuffer_Dequeue(&rx_queue, working_index + CRC_SIZE);
+
+			} else {
+				parser_statistics->crcs_invalid++;
+			}
+
+			working_index = 0;
+			working_segment_size = HEADER_SIZE;
+			parser_state = PARSER_STATE_HEADER;
+
+			if (valid_packet) {
+				return true;
+			}
+
+			break;
+		}
+
+		buffer_count = QueueBuffer_Count(&rx_queue);
+	}
+
+	return false;
+}

src/drivers/rc/crsf_rc/CrsfParser.hpp

@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+* @file CrsfParser.hpp
+*
+* Parser for incoming CRSF packets
+*
+* @author Chris Seto <chris1seto@gmail.com>
+*/
+
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#define CRSF_CHANNEL_COUNT 16
+
+struct CrsfChannelData_t {
+	float channels[CRSF_CHANNEL_COUNT];
+};
+
+struct CrsfLinkStatistics_t {
+	uint8_t uplink_rssi_1;
+	uint8_t uplink_rssi_2;
+	uint8_t uplink_link_quality;
+	int8_t uplink_snr;
+	uint8_t active_antenna;
+	uint8_t rf_mode;
+	uint8_t uplink_tx_power;
+	uint8_t downlink_rssi;
+	uint8_t downlink_link_quality;
+	int8_t downlink_snr;
+};
+
+struct CrsfParserStatistics_t {
+	uint32_t disposed_bytes;
+	uint32_t crcs_valid_known_packets;
+	uint32_t crcs_valid_unknown_packets;
+	uint32_t crcs_invalid;
+	uint32_t invalid_known_packet_sizes;
+};
+
+enum CRSF_MESSAGE_TYPE {
+	CRSF_MESSAGE_TYPE_RC_CHANNELS,
+	CRSF_MESSAGE_TYPE_LINK_STATISTICS,
+};
+
+typedef struct {
+	CRSF_MESSAGE_TYPE message_type;
+
+	union {
+		CrsfChannelData_t channel_data;
+		CrsfLinkStatistics_t link_statistics;
+	};
+} CrsfPacket_t;
+
+void CrsfParser_Init(void);
+bool CrsfParser_LoadBuffer(const uint8_t *buffer, const uint32_t size);
+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

@@ -32,12 +32,21 @@
  ****************************************************************************/
 
 #include "CrsfRc.hpp"
+#include "CrsfParser.hpp"
+#include "Crc8.hpp"
 
 #include <poll.h>
 #include <termios.h>
 
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/vehicle_status.h>
+
 using namespace time_literals;
 
+#define CRSF_BAUDRATE 420000
+
 CrsfRc::CrsfRc(const char *device) :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device))
@@ -81,10 +90,14 @@ int CrsfRc::task_spawn(int argc, char *argv[])
 	}
 
 	if (error_flag) {
-		return -1;
+		return PX4_ERROR;
+	}
+
+	if (!device_name) {
+		PX4_ERR("Valid device required");
+		return PX4_ERROR;
 	}
 
-	if (device_name && (access(device_name, R_OK | W_OK) == 0)) {
 	CrsfRc *instance = new CrsfRc(device_name);
 
 	if (instance == nullptr) {
@@ -98,17 +111,6 @@ int CrsfRc::task_spawn(int argc, char *argv[])
 	instance->ScheduleNow();
 
 	return PX4_OK;
-
-	} else {
-		if (device_name) {
-			PX4_ERR("invalid device (-d) %s", device_name);
-
-		} else {
-			PX4_ERR("valid device required");
-		}
-	}
-
-	return PX4_ERROR;
 }
 
 void CrsfRc::Run()
@@ -125,67 +127,101 @@ void CrsfRc::Run()
 		_rc_fd = ::open(_device, O_RDWR | O_NONBLOCK);
 
 		if (_rc_fd >= 0) {
-			// no parity, one stop bit
+			struct termios t;
-			struct termios t {};
+
 			tcgetattr(_rc_fd, &t);
 			cfsetspeed(&t, CRSF_BAUDRATE);
-			t.c_cflag &= ~(CSTOPB | PARENB);
+			t.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
+			t.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
+			t.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
+			t.c_oflag = 0;
 			tcsetattr(_rc_fd, TCSANOW, &t);
-		}
+
+			if (board_rc_swap_rxtx(_device)) {
+#if defined(TIOCSSWAP)
+				ioctl(_rc_fd, TIOCSSWAP, SER_SWAP_ENABLED);
+#endif // TIOCSSWAP
 			}
 
-	// poll with 3 second timeout
+			if (board_rc_singlewire(_device)) {
+				_is_singlewire = true;
+#if defined(TIOCSSINGLEWIRE)
+				ioctl(_rc_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
+#endif // TIOCSSINGLEWIRE
+			}
+
+			PX4_INFO("Crsf serial opened sucessfully");
+
+			if (_is_singlewire) {
+				PX4_INFO("Crsf serial is single wire. Telemetry disabled");
+			}
+
+			tcflush(_rc_fd, TCIOFLUSH);
+
+			Crc8Init(0xd5);
+		}
+
+		_input_rc.rssi_dbm = NAN;
+		_input_rc.link_quality = -1;
+
+		CrsfParser_Init();
+
+
+	}
+
+	// poll with 100mS timeout
 	pollfd fds[1];
 	fds[0].fd = _rc_fd;
 	fds[0].events = POLLIN;
-	int ret = ::poll(fds, 1, 3000);
+	int ret = ::poll(fds, 1, 100);
 
 	if (ret < 0) {
 		PX4_ERR("poll error");
 		// try again with delay
-		ScheduleDelayed(500_ms);
+		ScheduleDelayed(100_ms);
 		return;
 	}
 
 	const hrt_abstime time_now_us = hrt_absolute_time();
 	perf_count_interval(_cycle_interval_perf, time_now_us);
 
-	// read all available data from the serial RC input UART
+	// Read all available data from the serial RC input UART
 	int new_bytes = ::read(_rc_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE);
 
 	if (new_bytes > 0) {
 		_bytes_rx += new_bytes;
 
-		// parse new data
+		// Load new bytes into the CRSF parser buffer
-		uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {};
+		CrsfParser_LoadBuffer(_rcs_buf, new_bytes);
-		uint16_t raw_rc_count = 0;
-		bool rc_updated = crsf::crsf_parse(time_now_us, &_rcs_buf[0], new_bytes, &raw_rc_values[0], &raw_rc_count,
-						   input_rc_s::RC_INPUT_MAX_CHANNELS);
 
-		if (rc_updated) {
+		// Scan the parse buffer for messages, one at a time
-			if (!_rc_locked) {
+		CrsfPacket_t new_crsf_packet;
-				_rc_locked = true;
+
-				PX4_INFO("RC input locked");
+		while (CrsfParser_TryParseCrsfPacket(&new_crsf_packet, &_packet_parser_statistics)) {
+			switch (new_crsf_packet.message_type) {
+			case CRSF_MESSAGE_TYPE_RC_CHANNELS:
+				_input_rc.timestamp_last_signal = time_now_us;
+				_last_packet_seen = time_now_us;
+
+				for (int i = 0; i < CRSF_CHANNEL_COUNT; i++) {
+					_input_rc.values[i] = new_crsf_packet.channel_data.channels[i];
 				}
 
-			// we have a new CRSF frame. Publish it.
+				break;
-			input_rc_s input_rc{};
-			input_rc.timestamp_last_signal = time_now_us;
-			input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
-			input_rc.rssi = -1; // TODO
-			input_rc.rc_lost = (raw_rc_count == 0);
-			input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF;
 
-			for (int i = 0; i < input_rc.channel_count; i++) {
+			case CRSF_MESSAGE_TYPE_LINK_STATISTICS:
-				input_rc.values[i] = raw_rc_values[i];
+				_last_packet_seen = time_now_us;
+				_input_rc.rssi_dbm = -(float)new_crsf_packet.link_statistics.uplink_rssi_1;
+				_input_rc.link_quality = new_crsf_packet.link_statistics.uplink_link_quality;
+				break;
+
+			default:
+				break;
+			}
 		}
 
-			input_rc.timestamp = hrt_absolute_time();
+		if (_param_rc_crsf_tel_en.get() && !_is_singlewire
-			_input_rc_pub.publish(input_rc);
+		    && (_input_rc.timestamp > _telemetry_update_last + 100_ms)) {
-			perf_count(_publish_interval_perf);
-			_rc_valid = input_rc.timestamp;
-
-			if (_param_rc_crsf_tel_en.get() && (input_rc.timestamp > _telemetry_update_last + 100_ms)) {
 			switch (_next_type) {
 			case 0:
 				battery_status_s battery_status;
@@ -195,22 +231,22 @@ void CrsfRc::Run()
 					uint16_t current = battery_status.current_filtered_a * 10;
 					int fuel = battery_status.discharged_mah;
 					uint8_t remaining = battery_status.remaining * 100;
-						crsf::crsf_send_telemetry_battery(_rc_fd, voltage, current, fuel, remaining);
+					this->SendTelemetryBattery(voltage, current, fuel, remaining);
 				}
 
 				break;
 
 			case 1:
-					sensor_gps_s vehicle_gps_position;
+				sensor_gps_s sensor_gps;
 
-					if (_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
+				if (_vehicle_gps_position_sub.update(&sensor_gps)) {
-						int32_t latitude = vehicle_gps_position.lat;
+					int32_t latitude = sensor_gps.lat;
-						int32_t longitude = vehicle_gps_position.lon;
+					int32_t longitude = sensor_gps.lon;
-						uint16_t groundspeed = vehicle_gps_position.vel_d_m_s / 3.6f * 10.f;
+					uint16_t groundspeed = sensor_gps.vel_d_m_s / 3.6f * 10.f;
-						uint16_t gps_heading = math::degrees(vehicle_gps_position.cog_rad) * 100.f;
+					uint16_t gps_heading = math::degrees(sensor_gps.cog_rad) * 100.f;
-						uint16_t altitude = vehicle_gps_position.alt + 1000;
+					uint16_t altitude = sensor_gps.alt + 1000;
-						uint8_t num_satellites = vehicle_gps_position.satellites_used;
+					uint8_t num_satellites = sensor_gps.satellites_used;
-						crsf::crsf_send_telemetry_gps(_rc_fd, latitude, longitude, groundspeed, gps_heading, altitude, num_satellites);
+					this->SendTelemetryGps(latitude, longitude, groundspeed, gps_heading, altitude, num_satellites);
 				}
 
 				break;
@@ -223,7 +259,7 @@ void CrsfRc::Run()
 					int16_t pitch = attitude(1) * 1e4f;
 					int16_t roll = attitude(0) * 1e4f;
 					int16_t yaw = attitude(2) * 1e4f;
-						crsf::crsf_send_telemetry_attitude(_rc_fd, pitch, roll, yaw);
+					this->SendTelemetryAttitude(pitch, roll, yaw);
 				}
 
 				break;
@@ -264,6 +300,10 @@ void CrsfRc::Run()
 						flight_mode = "Auto";
 						break;
 
+					/*case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
+						flight_mode = "Failure";
+						break;*/
+
 					case vehicle_status_s::NAVIGATION_STATE_ACRO:
 						flight_mode = "Acro";
 						break;
@@ -279,29 +319,167 @@ void CrsfRc::Run()
 					case vehicle_status_s::NAVIGATION_STATE_STAB:
 						flight_mode = "Stabilized";
 						break;
+
+					default:
+						flight_mode = "Unknown";
 					}
 
-						crsf::crsf_send_telemetry_flight_mode(_rc_fd, flight_mode);
+					this->SendTelemetryFlightMode(flight_mode);
 				}
 
 				break;
 			}
 
-				_telemetry_update_last = input_rc.timestamp;
+			_telemetry_update_last = _input_rc.timestamp;
 			_next_type = (_next_type + 1) % num_data_types;
 		}
 	}
+
+	// If no communication
+	if (time_now_us - _last_packet_seen > 100_ms) {
+		// Invalidate link statistics
+		_input_rc.rssi_dbm = NAN;
+		_input_rc.link_quality = -1;
 	}
 
-	if (!_rc_locked && (hrt_elapsed_time(&_rc_valid) > 5_s)) {
+	// If we have not gotten RC updates specifically
-		::close(_rc_fd);
+	if (time_now_us - _input_rc.timestamp_last_signal > 50_ms) {
-		_rc_fd = -1;
+		_input_rc.rc_lost = 1;
-		_rc_locked = false;
+		_input_rc.rc_failsafe = 1;
-		ScheduleDelayed(200_ms);
 
 	} else {
+		_input_rc.rc_lost = 0;
+		_input_rc.rc_failsafe = 0;
+	}
+
+	_input_rc.channel_count = CRSF_CHANNEL_COUNT;
+	_input_rc.rssi = -1;
+	_input_rc.rc_ppm_frame_length = 0;
+	_input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF;
+	_input_rc.timestamp = hrt_absolute_time();
+	_input_rc_pub.publish(_input_rc);
+
+	perf_count(_publish_interval_perf);
+
 	ScheduleDelayed(4_ms);
 }
+
+/**
+ * write an uint8_t value to a buffer at a given offset and increment the offset
+ */
+static inline void write_uint8_t(uint8_t *buf, int &offset, uint8_t value)
+{
+	buf[offset++] = value;
+}
+
+/**
+ * write an uint16_t value to a buffer at a given offset and increment the offset
+ */
+static inline void write_uint16_t(uint8_t *buf, int &offset, uint16_t value)
+{
+	// Big endian
+	buf[offset] = value >> 8;
+	buf[offset + 1] = value & 0xff;
+	offset += 2;
+}
+
+/**
+ * write an uint24_t value to a buffer at a given offset and increment the offset
+ */
+static inline void write_uint24_t(uint8_t *buf, int &offset, int value)
+{
+	// Big endian
+	buf[offset] = value >> 16;
+	buf[offset + 1] = (value >> 8) & 0xff;
+	buf[offset + 2] = value & 0xff;
+	offset += 3;
+}
+
+/**
+ * write an int32_t value to a buffer at a given offset and increment the offset
+ */
+static inline void write_int32_t(uint8_t *buf, int &offset, int32_t value)
+{
+	// Big endian
+	buf[offset] = value >> 24;
+	buf[offset + 1] = (value >> 16) & 0xff;
+	buf[offset + 2] = (value >> 8) & 0xff;
+	buf[offset + 3] = value & 0xff;
+	offset += 4;
+}
+
+void CrsfRc::WriteFrameHeader(uint8_t *buf, int &offset, const crsf_frame_type_t type, const uint8_t payload_size)
+{
+	write_uint8_t(buf, offset, 0xc8); // this got changed from the address to the sync byte
+	write_uint8_t(buf, offset, payload_size + 2);
+	write_uint8_t(buf, offset, (uint8_t)type);
+}
+
+void CrsfRc::WriteFrameCrc(uint8_t *buf, int &offset, const int buf_size)
+{
+	// CRC does not include the address and length
+	write_uint8_t(buf, offset, Crc8Calc(buf + 2, buf_size - 3));
+}
+
+bool CrsfRc::SendTelemetryBattery(const uint16_t voltage, const uint16_t current, const int fuel,
+				  const uint8_t remaining)
+{
+	uint8_t buf[(uint8_t)crsf_payload_size_t::battery_sensor + 4];
+	int offset = 0;
+	WriteFrameHeader(buf, offset, crsf_frame_type_t::battery_sensor, (uint8_t)crsf_payload_size_t::battery_sensor);
+	write_uint16_t(buf, offset, voltage);
+	write_uint16_t(buf, offset, current);
+	write_uint24_t(buf, offset, fuel);
+	write_uint8_t(buf, offset, remaining);
+	WriteFrameCrc(buf, offset, sizeof(buf));
+	return write(_rc_fd, buf, offset) == offset;
+}
+
+bool CrsfRc::SendTelemetryGps(const int32_t latitude, const int32_t longitude, const uint16_t groundspeed,
+			      const uint16_t gps_heading, const uint16_t altitude, const uint8_t num_satellites)
+{
+	uint8_t buf[(uint8_t)crsf_payload_size_t::gps + 4];
+	int offset = 0;
+	WriteFrameHeader(buf, offset, crsf_frame_type_t::gps, (uint8_t)crsf_payload_size_t::gps);
+	write_int32_t(buf, offset, latitude);
+	write_int32_t(buf, offset, longitude);
+	write_uint16_t(buf, offset, groundspeed);
+	write_uint16_t(buf, offset, gps_heading);
+	write_uint16_t(buf, offset, altitude);
+	write_uint8_t(buf, offset, num_satellites);
+	WriteFrameCrc(buf, offset, sizeof(buf));
+	return write(_rc_fd, buf, offset) == offset;
+}
+
+bool CrsfRc::SendTelemetryAttitude(const int16_t pitch, const int16_t roll, const int16_t yaw)
+{
+	uint8_t buf[(uint8_t)crsf_payload_size_t::attitude + 4];
+	int offset = 0;
+	WriteFrameHeader(buf, offset, crsf_frame_type_t::attitude, (uint8_t)crsf_payload_size_t::attitude);
+	write_uint16_t(buf, offset, pitch);
+	write_uint16_t(buf, offset, roll);
+	write_uint16_t(buf, offset, yaw);
+	WriteFrameCrc(buf, offset, sizeof(buf));
+	return write(_rc_fd, buf, offset) == offset;
+}
+
+bool CrsfRc::SendTelemetryFlightMode(const char *flight_mode)
+{
+	const int max_length = 16;
+	int length = strlen(flight_mode) + 1;
+
+	if (length > max_length) {
+		length = max_length;
+	}
+
+	uint8_t buf[max_length + 4];
+	int offset = 0;
+	WriteFrameHeader(buf, offset, crsf_frame_type_t::flight_mode, length);
+	memcpy(buf + offset, flight_mode, length);
+	offset += length;
+	buf[offset - 1] = 0; // ensure null-terminated string
+	WriteFrameCrc(buf, offset, length + 4);
+	return write(_rc_fd, buf, offset) == offset;
 }
 
 int CrsfRc::print_status()
@@ -311,15 +489,22 @@ int CrsfRc::print_status()
 		PX4_INFO("UART RX bytes: %"  PRIu32, _bytes_rx);
 	}
 
-	PX4_INFO("RC state: %s", _rc_locked ? "found" : "searching for signal");
+	if (_is_singlewire) {
+		PX4_INFO("Telemetry disabled: Singlewire RC port");
 
-	if (_rc_locked) {
+	} else {
 		PX4_INFO("Telemetry: %s", _param_rc_crsf_tel_en.get() ? "yes" : "no");
 	}
 
 	perf_print_counter(_cycle_interval_perf);
 	perf_print_counter(_publish_interval_perf);
 
+	PX4_INFO("Disposed bytes: %li",  _packet_parser_statistics.disposed_bytes);
+	PX4_INFO("Valid known packet CRCs: %li",  _packet_parser_statistics.crcs_valid_known_packets);
+	PX4_INFO("Valid unknown packet CRCs: %li",  _packet_parser_statistics.crcs_valid_unknown_packets);
+	PX4_INFO("Invalid CRCs: %li",  _packet_parser_statistics.crcs_invalid);
+	PX4_INFO("Invalid known packet sizes: %li",  _packet_parser_statistics.invalid_known_packet_sizes);
+
 	return 0;
 }
 
@@ -337,7 +522,7 @@ int CrsfRc::print_usage(const char *reason)
 	PRINT_MODULE_DESCRIPTION(
 		R"DESCR_STR(
 ### Description
-This module does the CRSF RC input parsing.
+This module parses the CRSF RC uplink protocol and generates CRSF downlink telemetry data
 
 )DESCR_STR");
 

src/drivers/rc/crsf_rc/CrsfRc.hpp

@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include "crsf.h" // old parser TODO:
+#include "CrsfParser.hpp"
 
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/getopt.h>
@@ -74,18 +74,31 @@ public:
 private:
 	void Run() override;
 
-	hrt_abstime _rc_valid{0};
-	bool _rc_locked{false};
-
 	uORB::PublicationMulti<input_rc_s> _input_rc_pub{ORB_ID(input_rc)};
 
+	input_rc_s _input_rc{};
+
+	bool SendTelemetryBattery(const uint16_t voltage, const uint16_t current, const int fuel, const uint8_t remaining);
+
+	bool SendTelemetryGps(const int32_t latitude, const int32_t longitude, const uint16_t groundspeed,
+			      const uint16_t gps_heading, const uint16_t altitude, const uint8_t num_satellites);
+
+	bool SendTelemetryAttitude(const int16_t pitch, const int16_t roll, const int16_t yaw);
+
+	bool SendTelemetryFlightMode(const char *flight_mode);
+
 	int _rc_fd{-1};
 	char _device[20] {}; ///< device / serial port path
+	bool _is_singlewire{false};
 
 	static constexpr size_t RC_MAX_BUFFER_SIZE{64};
 	uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {};
 	uint32_t _bytes_rx{0};
 
+	hrt_abstime _last_packet_seen{0};
+
+	CrsfParserStatistics_t _packet_parser_statistics{0};
+
 	// telemetry
 	hrt_abstime _telemetry_update_last{0};
 	static constexpr int num_data_types{4}; ///< number of different telemetry data types
@@ -95,6 +108,34 @@ private:
 	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 
+	enum class crsf_frame_type_t : uint8_t {
+		gps = 0x02,
+		battery_sensor = 0x08,
+		link_statistics = 0x14,
+		rc_channels_packed = 0x16,
+		attitude = 0x1E,
+		flight_mode = 0x21,
+
+		// Extended Header Frames, range: 0x28 to 0x96
+		device_ping = 0x28,
+		device_info = 0x29,
+		parameter_settings_entry = 0x2B,
+		parameter_read = 0x2C,
+		parameter_write = 0x2D,
+		command = 0x32
+	};
+
+	enum class crsf_payload_size_t : uint8_t {
+		gps = 15,
+		battery_sensor = 8,
+		link_statistics = 10,
+		rc_channels = 22, ///< 11 bits per channel * 16 channels = 22 bytes.
+		attitude = 6,
+	};
+
+	void WriteFrameHeader(uint8_t *buf, int &offset, const crsf_frame_type_t type, const uint8_t payload_size);
+	void WriteFrameCrc(uint8_t *buf, int &offset, const int buf_size);
+
 	perf_counter_t	_cycle_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
 	perf_counter_t	_publish_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")};
 

src/drivers/rc/crsf_rc/QueueBuffer.cpp

@@ -0,0 +1,163 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+* @file QueueBuffer.cpp
+*
+* A very lightweight QueueBuffer implemtnation
+*
+* @author Chris Seto <chris1seto@gmail.com>
+*/
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+#include "QueueBuffer.hpp"
+
+void QueueBuffer_Init(QueueBuffer_t *const q, uint8_t *buffer, const uint32_t buffer_size)
+{
+	q->buffer = buffer;
+	q->buffer_size = buffer_size;
+
+	q->head = 0;
+	q->tail = 0;
+	q->count = 0;
+}
+
+uint32_t QueueBuffer_Count(const QueueBuffer_t *q)
+{
+	return q->count;
+}
+
+void QueueBuffer_Append(QueueBuffer_t *const q, const uint8_t x)
+{
+	// Append the data and make the new tail index
+	q->buffer[q->tail] = x;
+	q->tail = ((q->tail + 1) % q->buffer_size);
+
+	// Check if we can expand the count any more
+	if (q->count < q->buffer_size) {
+		q->count++;
+	}
+}
+
+bool QueueBuffer_AppendBuffer(QueueBuffer_t *const q, const uint8_t *x, const uint32_t append_size)
+{
+	uint32_t buffer_end_size;
+
+	buffer_end_size = q->buffer_size - q->tail;
+
+	// Check if we can even put this buffer into the queu
+	if (q->count + append_size > q->buffer_size) {
+		return false;
+	}
+
+	// Check if there is enough space on the end of the queue to put the entire buffer
+	if (buffer_end_size >= append_size) {
+		// We can write the entire append buffer in one shot
+		memcpy((void *)(q->buffer + q->tail), (void *)x, append_size);
+
+	} else {
+		memcpy((void *)(q->buffer + q->tail), (void *)x, buffer_end_size);
+		memcpy((void *)(q->buffer), (void *)(x + buffer_end_size), append_size - buffer_end_size);
+	}
+
+	// Append to the tail
+	q->tail = ((q->tail + append_size) % q->buffer_size);
+	q->count += append_size;
+
+	return true;
+}
+
+bool QueueBuffer_IsEmpty(const QueueBuffer_t *q)
+{
+	return (q->count == 0);
+}
+
+bool QueueBuffer_Get(QueueBuffer_t *const q, uint8_t *const x)
+{
+	if (q->count == 0) {
+		return false;
+	}
+
+	*x = q->buffer[q->head];
+	q->head = ((q->head + 1) % q->buffer_size);
+	q->count--;
+
+	return true;
+}
+
+void QueueBuffer_Dequeue(QueueBuffer_t *const q, const uint32_t n)
+{
+	if (n > q->count) {
+		return;
+	}
+
+	q->count -= n;
+	q->head = (q->head + n) % q->buffer_size;
+}
+
+bool QueueBuffer_Peek(const QueueBuffer_t *q, const uint32_t index, uint8_t *const x)
+{
+	if (index >= q->count) {
+		return false;
+	}
+
+	*x = q->buffer[(q->head + index) % q->buffer_size];
+	return true;
+}
+
+bool QueueBuffer_PeekBuffer(const QueueBuffer_t *q, const uint32_t index, uint8_t *buffer, const uint32_t size)
+{
+	uint32_t copy_start;
+
+	copy_start = q->head + index;
+
+	// Check to see if this amount of sizegth exists at the index
+	if (index + size > q->count) {
+		return false;
+	}
+
+	// Check if we can do this in a single shot
+	if (copy_start + size <= q->buffer_size) {
+		memcpy((void *)buffer, (void *)(q->buffer + copy_start), size);
+
+	} else {
+		// Double shot copy
+		uint32_t copy1Size = (q->buffer_size - copy_start);
+		memcpy((void *)buffer, (void *)(q->buffer + copy_start), copy1Size);
+		memcpy((void *)(buffer + copy1Size), (void *)(q->buffer), (size - copy1Size));
+	}
+
+	return true;
+}

src/drivers/rc/crsf_rc/QueueBuffer.hpp

@@ -0,0 +1,63 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file QueueBuffer.cpp
+ *
+ * A very lightweight QueueBuffer implemtnation
+ *
+ * @author Chris Seto <chris1seto@gmail.com>
+ */
+
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+typedef struct {
+	uint8_t *buffer;
+	uint32_t buffer_size;
+	uint32_t count;
+	uint32_t head;
+	uint32_t tail;
+} QueueBuffer_t;
+
+void QueueBuffer_Init(QueueBuffer_t *const q, uint8_t *buffer, const uint32_t buffer_size);
+uint32_t QueueBuffer_Count(const QueueBuffer_t *q);
+void QueueBuffer_Append(QueueBuffer_t *const q, const uint8_t x);
+bool QueueBuffer_AppendBuffer(QueueBuffer_t *const q, const uint8_t *x, const uint32_t append_size);
+bool QueueBuffer_IsEmpty(const QueueBuffer_t *q);
+bool QueueBuffer_Get(QueueBuffer_t *const q, uint8_t *const x);
+void QueueBuffer_Dequeue(QueueBuffer_t *const q, const uint32_t n);
+bool QueueBuffer_Peek(const QueueBuffer_t *q, const uint32_t index, uint8_t *const x);
+bool QueueBuffer_PeekBuffer(const QueueBuffer_t *q, const uint32_t index, uint8_t *buffer, const uint32_t size);

src/drivers/rc/crsf_rc/crsf.cpp

@@ -1,507 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2018-2022 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#if 0 // enable non-verbose debugging
-#define CRSF_DEBUG PX4_WARN
-#else
-#define CRSF_DEBUG(...)
-#endif
-
-#if 0 // verbose debugging. Careful when enabling: it leads to too much output, causing dropped bytes
-#define CRSF_VERBOSE PX4_WARN
-#else
-#define CRSF_VERBOSE(...)
-#endif
-
-#include <drivers/drv_hrt.h>
-#include <termios.h>
-#include <string.h>
-#include <unistd.h>
-
-#include "crsf.h"
-
-namespace crsf
-{
-
-#define CRSF_FRAME_SIZE_MAX 30 // the actual maximum length is 64, but we're only interested in RC channels and want to minimize buffer size
-#define CRSF_PAYLOAD_SIZE_MAX (CRSF_FRAME_SIZE_MAX-4)
-
-struct crsf_frame_header_t {
-	uint8_t device_address;             ///< @see crsf_address_t
-	uint8_t length;                     ///< length of crsf_frame_t (including CRC) minus sizeof(crsf_frame_header_t)
-};
-
-struct crsf_frame_t {
-	crsf_frame_header_t header;
-	uint8_t type;                       ///< @see crsf_frame_type_t
-	uint8_t payload[CRSF_PAYLOAD_SIZE_MAX + 1]; ///< payload data including 1 byte CRC at end
-};
-
-#define MIN(a,b) (((a)<(b))?(a):(b))
-#define MAX(a,b) (((a)>(b))?(a):(b))
-
-#define CRSF_SYNC_BYTE 0xC8
-
-enum class crsf_frame_type_t : uint8_t {
-	gps = 0x02,
-	battery_sensor = 0x08,
-	link_statistics = 0x14,
-	rc_channels_packed = 0x16,
-	attitude = 0x1E,
-	flight_mode = 0x21,
-
-	// Extended Header Frames, range: 0x28 to 0x96
-	device_ping = 0x28,
-	device_info = 0x29,
-	parameter_settings_entry = 0x2B,
-	parameter_read = 0x2C,
-	parameter_write = 0x2D,
-	command = 0x32
-};
-
-enum class crsf_payload_size_t : uint8_t {
-	gps = 15,
-	battery_sensor = 8,
-	link_statistics = 10,
-	rc_channels = 22, ///< 11 bits per channel * 16 channels = 22 bytes.
-	attitude = 6,
-};
-
-
-enum class crsf_address_t : uint8_t {
-	broadcast = 0x00,
-	usb = 0x10,
-	tbs_core_pnp_pro = 0x80,
-	reserved1 = 0x8A,
-	current_sensor = 0xC0,
-	gps = 0xC2,
-	tbs_blackbox = 0xC4,
-	flight_controller = 0xC8,
-	reserved2 = 0xCA,
-	race_tag = 0xCC,
-	radio_transmitter = 0xEA,
-	crsf_receiver = 0xEC,
-	crsf_transmitter = 0xEE
-};
-
-#pragma pack(push, 1)
-struct crsf_payload_RC_channels_packed_t {
-	// 176 bits of data (11 bits per channel * 16 channels) = 22 bytes
-	unsigned chan0 : 11;
-	unsigned chan1 : 11;
-	unsigned chan2 : 11;
-	unsigned chan3 : 11;
-	unsigned chan4 : 11;
-	unsigned chan5 : 11;
-	unsigned chan6 : 11;
-	unsigned chan7 : 11;
-	unsigned chan8 : 11;
-	unsigned chan9 : 11;
-	unsigned chan10 : 11;
-	unsigned chan11 : 11;
-	unsigned chan12 : 11;
-	unsigned chan13 : 11;
-	unsigned chan14 : 11;
-	unsigned chan15 : 11;
-};
-
-#pragma pack(pop)
-
-enum class crsf_parser_state_t : uint8_t {
-	unsynced = 0,
-	synced
-};
-
-static uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a)
-{
-	crc ^= a;
-
-	for (int i = 0; i < 8; ++i) {
-		if (crc & 0x80) {
-			crc = (crc << 1) ^ 0xD5;
-
-		} else {
-			crc = crc << 1;
-		}
-	}
-
-	return crc;
-}
-
-static uint8_t crc8_dvb_s2_buf(uint8_t *buf, int len)
-{
-	uint8_t crc = 0;
-
-	for (int i = 0; i < len; ++i) {
-		crc = crc8_dvb_s2(crc, buf[i]);
-	}
-
-	return crc;
-}
-
-static crsf_frame_t crsf_frame{};
-static unsigned current_frame_position = 0;
-static crsf_parser_state_t parser_state = crsf_parser_state_t::unsynced;
-
-/**
- * parse the current crsf_frame buffer
- */
-static bool crsf_parse_buffer(uint16_t *values, uint16_t *num_values, uint16_t max_channels);
-
-uint8_t crsf_frame_CRC(const crsf_frame_t &frame);
-
-/**
- * Convert from RC to PWM value
- * @param chan_value channel value in [172, 1811]
- * @return PWM channel value in [1000, 2000]
- */
-static uint16_t convert_channel_value(unsigned chan_value);
-
-
-bool crsf_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t *values,
-		uint16_t *num_values, uint16_t max_channels)
-{
-	bool ret = false;
-	uint8_t *crsf_frame_ptr = (uint8_t *)&crsf_frame;
-
-	while (len > 0) {
-
-		// fill in the crsf_buffer, as much as we can
-		const unsigned current_len = MIN(len, sizeof(crsf_frame_t) - current_frame_position);
-		memcpy(crsf_frame_ptr + current_frame_position, frame, current_len);
-		current_frame_position += current_len;
-
-		// protection to guarantee parsing progress
-		if (current_len == 0) {
-			CRSF_DEBUG("========== parser bug: no progress (%i) ===========", len);
-
-			for (unsigned i = 0; i < current_frame_position; ++i) {
-				CRSF_DEBUG("crsf_frame_ptr[%i]: 0x%x", i, (int)crsf_frame_ptr[i]);
-			}
-
-			// reset the parser
-			current_frame_position = 0;
-			parser_state = crsf_parser_state_t::unsynced;
-			return false;
-		}
-
-		len -= current_len;
-		frame += current_len;
-
-		if (crsf_parse_buffer(values, num_values, max_channels)) {
-			ret = true;
-		}
-	}
-
-
-	return ret;
-}
-
-uint8_t crsf_frame_CRC(const crsf_frame_t &frame)
-{
-	// CRC includes type and payload
-	uint8_t crc = crc8_dvb_s2(0, frame.type);
-
-	for (int i = 0; i < frame.header.length - 2; ++i) {
-		crc = crc8_dvb_s2(crc, frame.payload[i]);
-	}
-
-	return crc;
-}
-
-static uint16_t convert_channel_value(unsigned chan_value)
-{
-	/*
-	 *       RC     PWM
-	 * min  172 ->  988us
-	 * mid  992 -> 1500us
-	 * max 1811 -> 2012us
-	 */
-	static constexpr float scale = (2012.f - 988.f) / (1811.f - 172.f);
-	static constexpr float offset = 988.f - 172.f * scale;
-	return (scale * chan_value) + offset;
-}
-
-static bool crsf_parse_buffer(uint16_t *values, uint16_t *num_values, uint16_t max_channels)
-{
-	uint8_t *crsf_frame_ptr = (uint8_t *)&crsf_frame;
-
-	if (parser_state == crsf_parser_state_t::unsynced) {
-		// there is no sync byte, try to find an RC packet by searching for a matching frame length and type
-		for (unsigned i = 1; i < current_frame_position - 1; ++i) {
-			if (crsf_frame_ptr[i] == (uint8_t)crsf_payload_size_t::rc_channels + 2 &&
-			    crsf_frame_ptr[i + 1] == (uint8_t)crsf_frame_type_t::rc_channels_packed) {
-				parser_state = crsf_parser_state_t::synced;
-				unsigned frame_offset = i - 1;
-				CRSF_VERBOSE("RC channels found at offset %i", frame_offset);
-
-				// move the rest of the buffer to the beginning
-				if (frame_offset != 0) {
-					memmove(crsf_frame_ptr, crsf_frame_ptr + frame_offset, current_frame_position - frame_offset);
-					current_frame_position -= frame_offset;
-				}
-
-				break;
-			}
-		}
-	}
-
-	if (parser_state != crsf_parser_state_t::synced) {
-		if (current_frame_position >= sizeof(crsf_frame_t)) {
-			// discard most of the data, but keep the last 3 bytes (otherwise we could miss the frame start)
-			current_frame_position = 3;
-
-			for (unsigned i = 0; i < current_frame_position; ++i) {
-				crsf_frame_ptr[i] = crsf_frame_ptr[sizeof(crsf_frame_t) - current_frame_position + i];
-			}
-
-			CRSF_VERBOSE("Discarding buffer");
-		}
-
-		return false;
-	}
-
-
-	if (current_frame_position < 3) {
-		// wait until we have the header & type
-		return false;
-	}
-
-	// Now we have at least the header and the type
-
-	const unsigned current_frame_length = crsf_frame.header.length + sizeof(crsf_frame_header_t);
-
-	if (current_frame_length > sizeof(crsf_frame_t) || current_frame_length < 4) {
-		// frame too long or bogus -> discard everything and go into unsynced state
-		current_frame_position = 0;
-		parser_state = crsf_parser_state_t::unsynced;
-		CRSF_DEBUG("Frame too long/bogus (%i, type=%i) -> unsync", current_frame_length, crsf_frame.type);
-		return false;
-	}
-
-	if (current_frame_position < current_frame_length) {
-		// we don't have the full frame yet -> wait for more data
-		CRSF_VERBOSE("waiting for more data (%i < %i)", current_frame_position, current_frame_length);
-		return false;
-	}
-
-	bool ret = false;
-
-	// Now we have the full frame
-
-	if (crsf_frame.type == (uint8_t)crsf_frame_type_t::rc_channels_packed &&
-	    crsf_frame.header.length == (uint8_t)crsf_payload_size_t::rc_channels + 2) {
-		const uint8_t crc = crsf_frame.payload[crsf_frame.header.length - 2];
-
-		if (crc == crsf_frame_CRC(crsf_frame)) {
-			const crsf_payload_RC_channels_packed_t *const rc_channels =
-				(crsf_payload_RC_channels_packed_t *)&crsf_frame.payload;
-			*num_values = MIN(max_channels, 16);
-
-			if (max_channels > 0) { values[0] = convert_channel_value(rc_channels->chan0); }
-
-			if (max_channels > 1) { values[1] = convert_channel_value(rc_channels->chan1); }
-
-			if (max_channels > 2) { values[2] = convert_channel_value(rc_channels->chan2); }
-
-			if (max_channels > 3) { values[3] = convert_channel_value(rc_channels->chan3); }
-
-			if (max_channels > 4) { values[4] = convert_channel_value(rc_channels->chan4); }
-
-			if (max_channels > 5) { values[5] = convert_channel_value(rc_channels->chan5); }
-
-			if (max_channels > 6) { values[6] = convert_channel_value(rc_channels->chan6); }
-
-			if (max_channels > 7) { values[7] = convert_channel_value(rc_channels->chan7); }
-
-			if (max_channels > 8) { values[8] = convert_channel_value(rc_channels->chan8); }
-
-			if (max_channels > 9) { values[9] = convert_channel_value(rc_channels->chan9); }
-
-			if (max_channels > 10) { values[10] = convert_channel_value(rc_channels->chan10); }
-
-			if (max_channels > 11) { values[11] = convert_channel_value(rc_channels->chan11); }
-
-			if (max_channels > 12) { values[12] = convert_channel_value(rc_channels->chan12); }
-
-			if (max_channels > 13) { values[13] = convert_channel_value(rc_channels->chan13); }
-
-			if (max_channels > 14) { values[14] = convert_channel_value(rc_channels->chan14); }
-
-			if (max_channels > 15) { values[15] = convert_channel_value(rc_channels->chan15); }
-
-			CRSF_VERBOSE("Got Channels");
-
-			ret = true;
-
-		} else {
-			CRSF_DEBUG("CRC check failed");
-		}
-
-	} else {
-		CRSF_DEBUG("Got Non-RC frame (len=%i, type=%i)", current_frame_length, crsf_frame.type);
-		// We could check the CRC here and reset the parser into unsynced state if it fails.
-		// But in practise it's robust even without that.
-	}
-
-	// Either reset or move the rest of the buffer
-	if (current_frame_position > current_frame_length) {
-		CRSF_VERBOSE("Moving buffer (%i > %i)", current_frame_position, current_frame_length);
-		memmove(crsf_frame_ptr, crsf_frame_ptr + current_frame_length, current_frame_position - current_frame_length);
-		current_frame_position -= current_frame_length;
-
-	} else {
-		current_frame_position = 0;
-	}
-
-	return ret;
-}
-
-/**
- * write an uint8_t value to a buffer at a given offset and increment the offset
- */
-static inline void write_uint8_t(uint8_t *buf, int &offset, uint8_t value)
-{
-	buf[offset++] = value;
-}
-/**
- * write an uint16_t value to a buffer at a given offset and increment the offset
- */
-static inline void write_uint16_t(uint8_t *buf, int &offset, uint16_t value)
-{
-	// Big endian
-	buf[offset] = value >> 8;
-	buf[offset + 1] = value & 0xff;
-	offset += 2;
-}
-/**
- * write an uint24_t value to a buffer at a given offset and increment the offset
- */
-static inline void write_uint24_t(uint8_t *buf, int &offset, int value)
-{
-	// Big endian
-	buf[offset] = value >> 16;
-	buf[offset + 1] = (value >> 8) & 0xff;
-	buf[offset + 2] = value & 0xff;
-	offset += 3;
-}
-
-/**
- * write an int32_t value to a buffer at a given offset and increment the offset
- */
-static inline void write_int32_t(uint8_t *buf, int &offset, int32_t value)
-{
-	// Big endian
-	buf[offset] = value >> 24;
-	buf[offset + 1] = (value >> 16) & 0xff;
-	buf[offset + 2] = (value >> 8) & 0xff;
-	buf[offset + 3] = value & 0xff;
-	offset += 4;
-}
-
-static inline void write_frame_header(uint8_t *buf, int &offset, crsf_frame_type_t type, uint8_t payload_size)
-{
-	write_uint8_t(buf, offset, CRSF_SYNC_BYTE); // this got changed from the address to the sync byte
-	write_uint8_t(buf, offset, payload_size + 2);
-	write_uint8_t(buf, offset, (uint8_t)type);
-}
-static inline void write_frame_crc(uint8_t *buf, int &offset, int buf_size)
-{
-	// CRC does not include the address and length
-	write_uint8_t(buf, offset, crc8_dvb_s2_buf(buf + 2, buf_size - 3));
-
-	// check correctness of buffer size (only needed during development)
-	//if (buf_size != offset) { PX4_ERR("frame size mismatch (%i != %i)", buf_size, offset); }
-}
-
-bool crsf_send_telemetry_battery(int uart_fd, uint16_t voltage, uint16_t current, int fuel, uint8_t remaining)
-{
-	uint8_t buf[(uint8_t)crsf_payload_size_t::battery_sensor + 4];
-	int offset = 0;
-	write_frame_header(buf, offset, crsf_frame_type_t::battery_sensor, (uint8_t)crsf_payload_size_t::battery_sensor);
-	write_uint16_t(buf, offset, voltage);
-	write_uint16_t(buf, offset, current);
-	write_uint24_t(buf, offset, fuel);
-	write_uint8_t(buf, offset, remaining);
-	write_frame_crc(buf, offset, sizeof(buf));
-	return write(uart_fd, buf, offset) == offset;
-}
-
-bool crsf_send_telemetry_gps(int uart_fd, int32_t latitude, int32_t longitude, uint16_t groundspeed,
-			     uint16_t gps_heading, uint16_t altitude, uint8_t num_satellites)
-{
-	uint8_t buf[(uint8_t)crsf_payload_size_t::gps + 4];
-	int offset = 0;
-	write_frame_header(buf, offset, crsf_frame_type_t::gps, (uint8_t)crsf_payload_size_t::gps);
-	write_int32_t(buf, offset, latitude);
-	write_int32_t(buf, offset, longitude);
-	write_uint16_t(buf, offset, groundspeed);
-	write_uint16_t(buf, offset, gps_heading);
-	write_uint16_t(buf, offset, altitude);
-	write_uint8_t(buf, offset, num_satellites);
-	write_frame_crc(buf, offset, sizeof(buf));
-	return write(uart_fd, buf, offset) == offset;
-}
-
-bool crsf_send_telemetry_attitude(int uart_fd, int16_t pitch, int16_t roll, int16_t yaw)
-{
-	uint8_t buf[(uint8_t)crsf_payload_size_t::attitude + 4];
-	int offset = 0;
-	write_frame_header(buf, offset, crsf_frame_type_t::attitude, (uint8_t)crsf_payload_size_t::attitude);
-	write_uint16_t(buf, offset, pitch);
-	write_uint16_t(buf, offset, roll);
-	write_uint16_t(buf, offset, yaw);
-	write_frame_crc(buf, offset, sizeof(buf));
-	return write(uart_fd, buf, offset) == offset;
-}
-
-bool crsf_send_telemetry_flight_mode(int uart_fd, const char *flight_mode)
-{
-	const int max_length = 16;
-	int length = strlen(flight_mode) + 1;
-
-	if (length > max_length) {
-		length = max_length;
-	}
-
-	uint8_t buf[max_length + 4];
-	int offset = 0;
-	write_frame_header(buf, offset, crsf_frame_type_t::flight_mode, length);
-	memcpy(buf + offset, flight_mode, length);
-	offset += length;
-	buf[offset - 1] = 0; // ensure null-terminated string
-	write_frame_crc(buf, offset, length + 4);
-	return write(uart_fd, buf, offset) == offset;
-}
-
-}; // namespace crsf

src/drivers/rc/crsf_rc/crsf.h

@@ -1,112 +0,0 @@
-/****************************************************************************
- *
- *	Copyright (c) 2018 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *	notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *	notice, this list of conditions and the following disclaimer in
- *	the documentation and/or other materials provided with the
- *	distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *	used to endorse or promote products derived from this software
- *	without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file crsf.h
- *
- * RC protocol definition for CSRF (TBS Crossfire).
- * It is an uninverted protocol at 420000 baudrate.
- *
- * RC channels come in at 150Hz.
- *
- * @author Beat Küng <beat-kueng@gmx.net>
- */
-
-#pragma once
-
-#include <stdint.h>
-#include <px4_platform_common/defines.h>
-
-namespace crsf
-{
-
-#define CRSF_BAUDRATE 420000
-
-/**
- * Parse the CRSF protocol and extract RC channel data.
- *
- * @param now current time
- * @param frame data to parse
- * @param len length of frame
- * @param values output channel values, each in range [1000, 2000]
- * @param num_values set to the number of parsed channels in values
- * @param max_channels maximum length of values
- * @return true if channels successfully decoded
- */
-bool crsf_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t *values, uint16_t *num_values,
-		uint16_t max_channels);
-
-/**
- * Send telemetry battery information
- * @param uart_fd UART file descriptor
- * @param voltage Voltage [0.1V]
- * @param current Current [0.1A]
- * @param fuel drawn mAh
- * @param remaining battery remaining [%]
- * @return true on success
- */
-bool crsf_send_telemetry_battery(int uart_fd, uint16_t voltage, uint16_t current, int fuel, uint8_t remaining);
-
-/**
- * Send telemetry GPS information
- * @param uart_fd UART file descriptor
- * @param latitude latitude [degree * 1e7]
- * @param longitude longitude [degree * 1e7]
- * @param groundspeed Ground speed [km/h * 10]
- * @param gps_heading GPS heading [degree * 100]
- * @param altitude Altitude [meters + 1000m offset]
- * @param num_satellites number of satellites used
- * @return true on success
- */
-bool crsf_send_telemetry_gps(int uart_fd, int32_t latitude, int32_t longitude, uint16_t groundspeed,
-			     uint16_t gps_heading, uint16_t altitude, uint8_t num_satellites);
-
-/**
- * Send telemetry Attitude information
- * @param uart_fd UART file descriptor
- * @param pitch Pitch angle [rad * 1e4]
- * @param roll Roll angle [rad * 1e4]
- * @param yaw Yaw angle [rad * 1e4]
- * @return true on success
- */
-bool crsf_send_telemetry_attitude(int uart_fd, int16_t pitch, int16_t roll, int16_t yaw);
-
-/**
- * Send telemetry Flight Mode information
- * @param uart_fd UART file descriptor
- * @param flight_mode Flight Mode string (max length = 15)
- * @return true on success
- */
-bool crsf_send_telemetry_flight_mode(int uart_fd, const char *flight_mode);
-
-}; // namespace crsf

src/drivers/rc_input/RCInput.cpp

@@ -753,6 +753,9 @@ void RCInput::Run()
 		if (rc_updated) {
 			perf_count(_publish_interval_perf);
 
+			_rc_in.link_quality = -1;
+			_rc_in.rssi_dbm = NAN;
+
 			_to_input_rc.publish(_rc_in);
 
 		} else if (!rc_updated && !_armed && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) {

src/drivers/rpi_rc_in/rpi_rc_in.cpp

@@ -124,6 +124,8 @@ void RcInput::_measure(void)
 	_data.rc_failsafe = false;
 	_data.rc_lost = false;
 	_data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM;
+	_data.link_quality = -1;
+	_data.rssi_dbm = NAN;
 
 	_rcinput_pub.publish(_data);
 }

src/drivers/snapdragon_spektrum_rc/spektrum_rc.cpp

@@ -204,6 +204,9 @@ void fill_input_rc(uint16_t raw_rc_count, uint16_t raw_rc_values[input_rc_s::RC_
 	input_rc.rc_lost = (valid_chans == 0);
 	input_rc.rc_lost_frame_count = frame_drops;
 	input_rc.rc_total_frame_count = 0;
+
+	input_rc.link_quality = -1;
+	input_rc.rssi_dbm = NAN;
 }
 
 int start(int argc, char *argv[])

src/modules/mavlink/mavlink_receiver.cpp

@@ -2126,6 +2126,9 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg)
 		}
 	}
 
+	rc.link_quality = -1;
+	rc.rssi_dbm = NAN;
+
 	// publish uORB message
 	_rc_pub.publish(rc);
 }

src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp

@@ -913,6 +913,9 @@ void SimulatorMavlink::handle_message_rc_channels(const mavlink_message_t *msg)
 	rc_input.values[16] = rc_channels.chan17_raw;
 	rc_input.values[17] = rc_channels.chan18_raw;
 
+	rc_input.link_quality = -1;
+	rc_input.rssi_dbm = NAN;
+
 	rc_input.timestamp = hrt_absolute_time();
 
 	// publish message