fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

[crsf_rc] Add support for link statistic messages

Browse Source
This commit is contained in:
Niklas Hauser 2025-09-17 00:27:18 +08:00 committed by Niklas Hauser
parent 6901bc6a01
commit a514560169
6 changed files with 111 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
docs/en/releases/main.md
View File

@ -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)).

2
msg/InputRc.msg
View File

@ -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

68
src/drivers/rc/crsf_rc/CrsfParser.cpp
View File

@ -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

20
src/drivers/rc/crsf_rc/CrsfParser.hpp
View File

@ -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;

31
src/drivers/rc/crsf_rc/CrsfRc.cpp
View File

@ -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();

1
src/drivers/rc/crsf_rc/CrsfRc.hpp
View File

@ -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};