From a5145601699dfd7d3160c7a4ac41d8c75aa1144d Mon Sep 17 00:00:00 2001 From: Niklas Hauser Date: Wed, 17 Sep 2025 00:27:18 +0800 Subject: [PATCH] [crsf_rc] Add support for link statistic messages --- docs/en/releases/main.md | 4 ++ msg/InputRc.msg | 2 + src/drivers/rc/crsf_rc/CrsfParser.cpp | 68 ++++++++++++++++++++++----- src/drivers/rc/crsf_rc/CrsfParser.hpp | 20 ++++++++ src/drivers/rc/crsf_rc/CrsfRc.cpp | 31 +++++++++++- src/drivers/rc/crsf_rc/CrsfRc.hpp | 1 + 6 files changed, 111 insertions(+), 15 deletions(-) diff --git a/docs/en/releases/main.md b/docs/en/releases/main.md index 3931d565b4..a8c1805fbb 100644 --- a/docs/en/releases/main.md +++ b/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)). diff --git a/msg/InputRc.msg b/msg/InputRc.msg index 782477407e..2f72125ab8 100644 --- a/msg/InputRc.msg +++ b/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 diff --git a/src/drivers/rc/crsf_rc/CrsfParser.cpp b/src/drivers/rc/crsf_rc/CrsfParser.cpp index a81fe930f5..61d2029b0d 100644 --- a/src/drivers/rc/crsf_rc/CrsfParser.cpp +++ b/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[CRSF_PACKET_DESCRIPTOR_COUNT] = { +static const CrsfPacketDescriptor_t crsf_packet_descriptors[] = { {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) { - 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; - } + if (working_descriptor->packet_size == -1) { + working_segment_size = packet_size - PACKET_SIZE_TYPE_SIZE; - 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 diff --git a/src/drivers/rc/crsf_rc/CrsfParser.hpp b/src/drivers/rc/crsf_rc/CrsfParser.hpp index c0bf0e8494..167cd6466e 100644 --- a/src/drivers/rc/crsf_rc/CrsfParser.hpp +++ b/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; diff --git a/src/drivers/rc/crsf_rc/CrsfRc.cpp b/src/drivers/rc/crsf_rc/CrsfRc.cpp index 9e311da18e..f099fff909 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.cpp +++ b/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(); diff --git a/src/drivers/rc/crsf_rc/CrsfRc.hpp b/src/drivers/rc/crsf_rc/CrsfRc.hpp index c3b0a4ec54..a47b361903 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.hpp +++ b/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};