From 2bf50badf415a2f2f2f9608529a9542417537102 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 17 Mar 2026 19:11:33 -0800 Subject: [PATCH] feat(crsf_rc): add CRSF receiver bind command Add ability to initiate CRSF receiver binding from QGroundControl or the NSH console. When MAV_CMD_START_RX_PAIR is received with RC_TYPE_CRSF, the driver sends the CRSF bind command frame over UART. Binding is rejected when armed or on singlewire configurations. Also adds RC_TYPE and RC_SUB_TYPE constants to VehicleCommand.msg and replaces magic numbers in DsmRc and RCInput drivers. Based on PX4/PX4-Autopilot#23294. --- msg/versioned/VehicleCommand.msg | 7 +++ src/drivers/rc/crsf_rc/CrsfRc.cpp | 74 ++++++++++++++++++++++++++++--- src/drivers/rc/crsf_rc/CrsfRc.hpp | 18 ++++++++ src/drivers/rc/dsm_rc/DsmRc.cpp | 8 ++-- src/drivers/rc_input/RCInput.cpp | 8 ++-- 5 files changed, 102 insertions(+), 13 deletions(-) diff --git a/msg/versioned/VehicleCommand.msg b/msg/versioned/VehicleCommand.msg index 6ea47acc27..898938d8f1 100644 --- a/msg/versioned/VehicleCommand.msg +++ b/msg/versioned/VehicleCommand.msg @@ -180,6 +180,13 @@ uint8 ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE = 3 uint8 ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED = 4 uint8 ORBIT_YAW_BEHAVIOUR_UNCHANGED = 5 +# Used as param1&2 in CMD_START_RX_PAIR. +uint8 RC_TYPE_SPEKTRUM = 0 +uint8 RC_TYPE_CRSF = 1 +uint8 RC_SUB_TYPE_SPEKTRUM_DSM2 = 0 +uint8 RC_SUB_TYPE_SPEKTRUM_DSMX = 1 +uint8 RC_SUB_TYPE_SPEKTRUM_DSMX8 = 2 + # Used as param1 in ARM_DISARM command. int8 ARMING_ACTION_DISARM = 0 int8 ARMING_ACTION_ARM = 1 diff --git a/src/drivers/rc/crsf_rc/CrsfRc.cpp b/src/drivers/rc/crsf_rc/CrsfRc.cpp index 6f73c3f7d2..b6cdf4b72e 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.cpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.cpp @@ -37,11 +37,6 @@ #include -#include -#include -#include -#include - using namespace time_literals; ModuleBase::Descriptor CrsfRc::desc{task_spawn, custom_command, print_usage}; @@ -192,6 +187,47 @@ void CrsfRc::Run() const hrt_abstime time_now_us = hrt_absolute_time(); perf_count_interval(_cycle_interval_perf, time_now_us); + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + _armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + } + } + + /* vehicle command */ + vehicle_command_s vcmd; + + if (_vehicle_cmd_sub.update(&vcmd)) { + // Check for a pairing command + if (vcmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) { + + uint8_t cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; + + if (!_is_singlewire && !_armed) { + if ((int)vcmd.param1 == vehicle_command_s::RC_TYPE_CRSF) { + // CRSF binding command + if (BindCRSF()) { + cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + } + } + + } else { + cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + // publish acknowledgement + vehicle_command_ack_s command_ack{}; + command_ack.command = vcmd.command; + command_ack.result = cmd_ret; + command_ack.target_system = vcmd.source_system; + command_ack.target_component = vcmd.source_component; + command_ack.timestamp = hrt_absolute_time(); + uORB::Publication vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)}; + vehicle_command_ack_pub.publish(command_ack); + } + } + // Read all available data from the serial RC input UART int new_bytes = _uart->readAtLeast(&_rcs_buf[0], RC_MAX_BUFFER_SIZE, 1, 100); @@ -518,6 +554,23 @@ bool CrsfRc::SendTelemetryFlightMode(const char *flight_mode) return _uart->write((void *) buf, (size_t) offset); } +bool CrsfRc::BindCRSF() +{ + uint8_t bind_frame[] = { + 0xC8, // sync + 0x07, // frame length + (uint8_t)crsf_frame_type_t::command, + (uint8_t)crsf_address_t::crsf_receiver, + (uint8_t)crsf_address_t::flight_controller, + (uint8_t)crsf_sub_command_t::subcmd_rx, + (uint8_t)crsf_sub_command_t::subcmd_rx_bind, + 0x9E, // command CRC8 + 0xE8, // packet CRC8 + }; + + return _uart->write((void *)bind_frame, sizeof(bind_frame)); +} + int CrsfRc::print_status() { if (_device[0] != '\0') { @@ -547,6 +600,16 @@ int CrsfRc::print_status() int CrsfRc::custom_command(int argc, char *argv[]) { + if (!strcmp(argv[0], "bind")) { + uORB::Publication vehicle_command_pub{ORB_ID(vehicle_command)}; + vehicle_command_s vcmd{}; + vcmd.command = vehicle_command_s::VEHICLE_CMD_START_RX_PAIR; + vcmd.param1 = vehicle_command_s::RC_TYPE_CRSF; + vcmd.timestamp = hrt_absolute_time(); + vehicle_command_pub.publish(vcmd); + return 0; + } + #ifdef CONFIG_RC_CRSF_INJECT if (!strcmp(argv[0], "start")) { @@ -604,6 +667,7 @@ This module parses the CRSF RC uplink protocol and generates CRSF downlink telem PRINT_MODULE_USAGE_SUBCATEGORY("radio_control"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "", "RC device", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("bind", "Send a CRSF bind command (not available on singlewire)"); #ifdef CONFIG_RC_CRSF_INJECT PRINT_MODULE_USAGE_COMMAND_DESCR("inject", "Inject frame data bytes (for testing)"); #endif diff --git a/src/drivers/rc/crsf_rc/CrsfRc.hpp b/src/drivers/rc/crsf_rc/CrsfRc.hpp index 85cda4e0ab..c29a2e8768 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.hpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.hpp @@ -53,6 +53,8 @@ #include #include #include +#include +#include using namespace device; @@ -92,10 +94,13 @@ private: bool SendTelemetryFlightMode(const char *flight_mode); + bool BindCRSF(); + Serial *_uart = nullptr; ///< UART interface to RC char _device[20] {}; ///< device / serial port path bool _is_singlewire{false}; + bool _armed{false}; static constexpr size_t RC_MAX_BUFFER_SIZE{64}; uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {}; @@ -114,6 +119,7 @@ private: uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_cmd_sub{ORB_ID(vehicle_command)}; enum class crsf_frame_type_t : uint8_t { gps = 0x02, @@ -140,6 +146,18 @@ private: attitude = 6, }; + enum class crsf_address_t : uint8_t { + broadcast = 0x00, + flight_controller = 0xC8, + crsf_receiver = 0xEC, + crsf_transmitter = 0xEE + }; + + enum class crsf_sub_command_t : uint8_t { + subcmd_rx = 0x10, + subcmd_rx_bind = 0x01, + }; + 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); diff --git a/src/drivers/rc/dsm_rc/DsmRc.cpp b/src/drivers/rc/dsm_rc/DsmRc.cpp index 257e90c569..0dbe59ab79 100644 --- a/src/drivers/rc/dsm_rc/DsmRc.cpp +++ b/src/drivers/rc/dsm_rc/DsmRc.cpp @@ -166,19 +166,19 @@ void DsmRc::Run() #if defined(SPEKTRUM_POWER) if (!_rc_scan_locked && !_armed) { - if ((int)vcmd.param1 == 0) { + if ((int)vcmd.param1 == vehicle_command_s::RC_TYPE_SPEKTRUM) { // DSM binding command int dsm_bind_mode = (int)vcmd.param2; int dsm_bind_pulses = 0; - if (dsm_bind_mode == 0) { + if (dsm_bind_mode == vehicle_command_s::RC_SUB_TYPE_SPEKTRUM_DSM2) { dsm_bind_pulses = DSM2_BIND_PULSES; - } else if (dsm_bind_mode == 1) { + } else if (dsm_bind_mode == vehicle_command_s::RC_SUB_TYPE_SPEKTRUM_DSMX) { dsm_bind_pulses = DSMX_BIND_PULSES; - } else { + } else if (dsm_bind_mode == vehicle_command_s::RC_SUB_TYPE_SPEKTRUM_DSMX8) { dsm_bind_pulses = DSMX8_BIND_PULSES; } diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index 6926621edf..47c4113723 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -388,19 +388,19 @@ void RCInput::Run() #if defined(SPEKTRUM_POWER) if (!_rc_scan_locked && !_armed) { - if ((int)vcmd.param1 == 0) { + if ((int)vcmd.param1 == vehicle_command_s::RC_TYPE_SPEKTRUM) { // DSM binding command int dsm_bind_mode = (int)vcmd.param2; int dsm_bind_pulses = 0; - if (dsm_bind_mode == 0) { + if (dsm_bind_mode == vehicle_command_s::RC_SUB_TYPE_SPEKTRUM_DSM2) { dsm_bind_pulses = DSM2_BIND_PULSES; - } else if (dsm_bind_mode == 1) { + } else if (dsm_bind_mode == vehicle_command_s::RC_SUB_TYPE_SPEKTRUM_DSMX) { dsm_bind_pulses = DSMX_BIND_PULSES; - } else { + } else if (dsm_bind_mode == vehicle_command_s::RC_SUB_TYPE_SPEKTRUM_DSMX8) { dsm_bind_pulses = DSMX8_BIND_PULSES; }