[crsf_rc] Add support for link statistic messages

docs/en/releases/main.md

@@ -76,6 +76,10 @@ Please continue reading for [upgrade instructions](#upgrade-guide).
 
 - TBD
 
+### RC
+
+- Parse ELRS Status and Link Statistics TX messages in the CRSF parser.
+
 ### Multi-Rotor
 
 - Removed parameters `MPC_{XY/Z/YAW}_MAN_EXPO` and use default value instead, as they were not deemed necessary anymore. ([PX4-Autopilot#25435: Add new flight mode: Altitude Cruise](https://github.com/PX4/PX4-Autopilot/pull/25435)).

msg/InputRc.msg

@@ -32,9 +32,11 @@ bool rc_lost				# RC receiver connection status: True,if no frame has arrived in
 uint16 rc_lost_frame_count		# Number of lost RC frames. Note: intended purpose: observe the radio link quality if RSSI is not available. This value must not be used to trigger any failsafe-alike functionality.
 uint16 rc_total_frame_count		# Number of total RC frames. Note: intended purpose: observe the radio link quality if RSSI is not available. This value must not be used to trigger any failsafe-alike functionality.
 uint16 rc_ppm_frame_length		# Length of a single PPM frame. Zero for non-PPM systems
+uint16 rc_frame_rate			# RC frame rate in msg/second. 0 = invalid
 
 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
+int8 link_snr				# link signal to noise ratio in units of dB. -1 = invalid

src/drivers/rc/crsf_rc/CrsfParser.cpp

@@ -57,8 +57,10 @@ enum CRSF_PAYLOAD_SIZE {
 	CRSF_PAYLOAD_SIZE_GPS = 15,
 	CRSF_PAYLOAD_SIZE_BATTERY = 8,
 	CRSF_PAYLOAD_SIZE_LINK_STATISTICS = 10,
+	CRSF_PAYLOAD_SIZE_LINK_STATISTICS_TX = -1,
 	CRSF_PAYLOAD_SIZE_RC_CHANNELS = 22,
 	CRSF_PAYLOAD_SIZE_ATTITUDE = 6,
+	CRSF_PAYLOAD_SIZE_ELRS_STATUS = -1, // unclear how large this message is
 };
 
 enum CRSF_PACKET_TYPE {
@@ -68,6 +70,8 @@ enum CRSF_PACKET_TYPE {
 	CRSF_PACKET_TYPE_OPENTX_SYNC = 0x10,
 	CRSF_PACKET_TYPE_RADIO_ID = 0x3A,
 	CRSF_PACKET_TYPE_RC_CHANNELS_PACKED = 0x16,
+	CRSF_PACKET_TYPE_LINK_STATISTICS_RX = 0x1C,
+	CRSF_PACKET_TYPE_LINK_STATISTICS_TX = 0x1D,
 	CRSF_PACKET_TYPE_ATTITUDE = 0x1E,
 	CRSF_PACKET_TYPE_FLIGHT_MODE = 0x21,
 	// Extended Header Frames, range: 0x28 to 0x96
@@ -76,6 +80,7 @@ enum CRSF_PACKET_TYPE {
 	CRSF_PACKET_TYPE_PARAMETER_SETTINGS_ENTRY = 0x2B,
 	CRSF_PACKET_TYPE_PARAMETER_READ = 0x2C,
 	CRSF_PACKET_TYPE_PARAMETER_WRITE = 0x2D,
+	CRSF_PACKET_TYPE_ELRS_STATUS = 0x2E,
 	CRSF_PACKET_TYPE_COMMAND = 0x32,
 	// MSP commands
 	CRSF_PACKET_TYPE_MSP_REQ = 0x7A,   // response request using msp sequence as command
@@ -114,18 +119,22 @@ enum PARSER_STATE {
 
 typedef struct {
 	uint8_t packet_type;
-	uint32_t packet_size;
+	int32_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);
+static bool ProcessLinkStatisticsTx(const uint8_t *data, const uint32_t size, CrsfPacket_t *const new_packet);
+static bool ProcessElrsStatus(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[] = {
-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},
+	{CRSF_PACKET_TYPE_LINK_STATISTICS_TX, CRSF_PAYLOAD_SIZE_LINK_STATISTICS_TX, ProcessLinkStatisticsTx},
+	{CRSF_PACKET_TYPE_ELRS_STATUS, CRSF_PAYLOAD_SIZE_ELRS_STATUS, ProcessElrsStatus},
 };
+#define CRSF_PACKET_DESCRIPTOR_COUNT  (sizeof(crsf_packet_descriptors) / sizeof(CrsfPacketDescriptor_t))
 
 static enum PARSER_STATE parser_state = PARSER_STATE_HEADER;
 static uint32_t working_index = 0;
@@ -201,7 +210,7 @@ static bool ProcessLinkStatistics(const uint8_t *data, const uint32_t size, Crsf
 	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_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];
@@ -214,6 +223,34 @@ static bool ProcessLinkStatistics(const uint8_t *data, const uint32_t size, Crsf
 	return true;
 }
 
+static bool ProcessLinkStatisticsTx(const uint8_t *data, const uint32_t size, CrsfPacket_t *const new_packet)
+{
+	new_packet->message_type = CRSF_MESSAGE_TYPE_LINK_STATISTICS_TX;
+
+	new_packet->link_statistics_tx.uplink_rssi = data[0];
+	new_packet->link_statistics_tx.uplink_rssi_pct = data[1];
+	new_packet->link_statistics_tx.uplink_link_quality = data[2];
+	new_packet->link_statistics_tx.uplink_snr = data[3];
+	new_packet->link_statistics_tx.downlink_power = data[4];
+	new_packet->link_statistics_tx.uplink_fps = data[5];
+
+	return true;
+}
+
+static bool ProcessElrsStatus(const uint8_t *data, const uint32_t size, CrsfPacket_t *const new_packet)
+{
+	new_packet->message_type = CRSF_MESSAGE_TYPE_ELRS_STATUS;
+
+	// Try: crsf_rc inject 0x2E 0x13 0x50 0xFB 0x53 0x31 0x63 0x63 0x63 0x63
+
+	new_packet->elrs_status.packets_bad = data[2];
+	new_packet->elrs_status.packets_good = (data[3] << 8) | data[4];
+	new_packet->elrs_status.flags = data[5];
+	strlcpy(new_packet->elrs_status.message, (const char *)&data[6], sizeof(new_packet->elrs_status.message));
+
+	return true;
+}
+
 static CrsfPacketDescriptor_t *FindCrsfDescriptor(const enum CRSF_PACKET_TYPE packet_type)
 {
 	uint32_t i;
@@ -280,16 +317,21 @@ bool CrsfParser_TryParseCrsfPacket(CrsfPacket_t *const new_packet, CrsfParserSta
 			// If we know what this packet is...
 			if (working_descriptor != NULL) {
 				// Validate length
-				if (packet_size != working_descriptor->packet_size + PACKET_SIZE_TYPE_SIZE) {
+				if (working_descriptor->packet_size == -1) {
-					parser_statistics->invalid_known_packet_sizes++;
+					working_segment_size = packet_size - PACKET_SIZE_TYPE_SIZE;
-					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 {
+					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

src/drivers/rc/crsf_rc/CrsfParser.hpp

@@ -63,6 +63,22 @@ struct CrsfLinkStatistics_t {
 	int8_t downlink_snr;
 };
 
+struct CrsfLinkStatisticsTx_t {
+	uint8_t uplink_rssi;
+	uint8_t uplink_rssi_pct;
+	uint8_t uplink_link_quality;
+	int8_t uplink_snr;
+	uint8_t downlink_power;
+	uint8_t uplink_fps;
+};
+
+struct CrsfElrsStatus_t {
+	uint8_t packets_bad;
+	uint16_t packets_good;
+	uint8_t flags;
+	char message[48];
+};
+
 struct CrsfParserStatistics_t {
 	uint32_t disposed_bytes;
 	uint32_t crcs_valid_known_packets;
@@ -75,6 +91,8 @@ struct CrsfParserStatistics_t {
 enum CRSF_MESSAGE_TYPE {
 	CRSF_MESSAGE_TYPE_RC_CHANNELS,
 	CRSF_MESSAGE_TYPE_LINK_STATISTICS,
+	CRSF_MESSAGE_TYPE_LINK_STATISTICS_TX,
+	CRSF_MESSAGE_TYPE_ELRS_STATUS,
 };
 
 typedef struct {
@@ -83,6 +101,8 @@ typedef struct {
 	union {
 		CrsfChannelData_t channel_data;
 		CrsfLinkStatistics_t link_statistics;
+		CrsfLinkStatisticsTx_t link_statistics_tx;
+		CrsfElrsStatus_t elrs_status;
 	};
 } CrsfPacket_t;
 

src/drivers/rc/crsf_rc/CrsfRc.cpp

@@ -178,8 +178,11 @@ void CrsfRc::Run()
 
 		Crc8Init(0xd5);
 
+		_input_rc.rssi = -1;
 		_input_rc.rssi_dbm = NAN;
 		_input_rc.link_quality = -1;
+		_input_rc.rc_frame_rate = 0;
+		_input_rc.link_snr = -1;
 
 		CrsfParser_Init();
 	}
@@ -213,8 +216,30 @@ void CrsfRc::Run()
 
 			case CRSF_MESSAGE_TYPE_LINK_STATISTICS:
 				_last_packet_seen = time_now_us;
-				_input_rc.rssi_dbm = -(float)new_crsf_packet.link_statistics.uplink_rssi_1;
+				_input_rc.rssi_dbm = -(float)(new_crsf_packet.link_statistics.active_antenna ?
+							      new_crsf_packet.link_statistics.uplink_rssi_2 :
+							      new_crsf_packet.link_statistics.uplink_rssi_1);
+
+				if (time_now_us - _last_stats_tx_seen > 500_ms) {
+					// We haven't received link statistics tx in a while, use an approximation
+					_input_rc.rssi = (1.f - _input_rc.rssi_dbm / -130.f) * _input_rc.RSSI_MAX;
+				}
+
 				_input_rc.link_quality = new_crsf_packet.link_statistics.uplink_link_quality;
+				_input_rc.rc_frame_rate = new_crsf_packet.link_statistics.rf_mode;
+				_input_rc.link_snr = new_crsf_packet.link_statistics.uplink_snr;
+				break;
+
+			case CRSF_MESSAGE_TYPE_LINK_STATISTICS_TX:
+				_last_packet_seen = time_now_us;
+				_last_stats_tx_seen = time_now_us;
+				_input_rc.rssi = new_crsf_packet.link_statistics_tx.uplink_rssi_pct;
+				break;
+
+			case CRSF_MESSAGE_TYPE_ELRS_STATUS:
+				_last_packet_seen = time_now_us;
+				_input_rc.rc_lost_frame_count = new_crsf_packet.elrs_status.packets_bad;
+				_input_rc.rc_total_frame_count = new_crsf_packet.elrs_status.packets_good;
 				break;
 
 			default:
@@ -344,8 +369,11 @@ void CrsfRc::Run()
 	// If no communication
 	if (time_now_us - _last_packet_seen > 100_ms) {
 		// Invalidate link statistics
+		_input_rc.rssi = -1;
 		_input_rc.rssi_dbm = NAN;
 		_input_rc.link_quality = -1;
+		_input_rc.rc_frame_rate = 0;
+		_input_rc.link_snr = -1;
 	}
 
 	// If we have not gotten RC updates specifically
@@ -359,7 +387,6 @@ void CrsfRc::Run()
 	}
 
 	_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();

src/drivers/rc/crsf_rc/CrsfRc.hpp

@@ -100,6 +100,7 @@ private:
 	uint32_t _bytes_rx{0};
 
 	hrt_abstime _last_packet_seen{0};
+	hrt_abstime _last_stats_tx_seen{0};
 
 	CrsfParserStatistics_t _packet_parser_statistics{0};