From f668029f751e6de3cc7e29bec80b91aae2c73b94 Mon Sep 17 00:00:00 2001 From: benjinne Date: Tue, 18 Jun 2024 10:22:55 -0400 Subject: [PATCH 1/5] crsf_rc: add bind command --- src/drivers/rc/crsf_rc/CrsfRc.cpp | 68 ++++++++++++++++++++++++++++--- src/drivers/rc/crsf_rc/CrsfRc.hpp | 32 +++++++++++++++ 2 files changed, 95 insertions(+), 5 deletions(-) diff --git a/src/drivers/rc/crsf_rc/CrsfRc.cpp b/src/drivers/rc/crsf_rc/CrsfRc.cpp index 3ea6c0ac273d..b42ea714fffe 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; #define CRSF_BAUDRATE 420000 @@ -182,6 +177,40 @@ 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 (!_armed) { + BindCRSF(); + cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + } + + // 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); @@ -480,6 +509,23 @@ bool CrsfRc::SendTelemetryFlightMode(const char *flight_mode) return _uart->write((void *) buf, (size_t) offset); } +bool CrsfRc::BindCRSF() +{ + uint8_t bindFrame[] = { + (uint8_t)crsf_sync_t::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 *) bindFrame, 9); +} + int CrsfRc::print_status() { if (_device[0] != '\0') { @@ -509,6 +555,17 @@ int CrsfRc::print_status() int CrsfRc::custom_command(int argc, char *argv[]) { + const char *verb = argv[0]; + + if (!strcmp(verb, "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.timestamp = hrt_absolute_time(); + vehicle_command_pub.publish(vcmd); + return 0; + } + return print_usage("unknown command"); } @@ -528,6 +585,7 @@ This module parses the CRSF RC uplink protocol and generates CRSF downlink telem PRINT_MODULE_USAGE_NAME("crsf_rc", "driver"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "", "RC device", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("bind", "Send a bind command (not available on singlewire)"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); diff --git a/src/drivers/rc/crsf_rc/CrsfRc.hpp b/src/drivers/rc/crsf_rc/CrsfRc.hpp index c3b0a4ec54d0..8f6a98f08c2b 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; @@ -90,10 +92,13 @@ class CrsfRc : public ModuleBase, public ModuleParams, public px4::Sched 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] {}; @@ -111,6 +116,7 @@ class CrsfRc : public ModuleBase, public ModuleParams, public px4::Sched 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, @@ -137,6 +143,32 @@ class CrsfRc : public ModuleBase, public ModuleParams, public px4::Sched attitude = 6, }; + enum class crsf_sync_t : uint8_t { + sync = 0xC8 + }; + + enum class crsf_sub_command_t : uint8_t { + subcmd_rx = 0x10, + subcmd_general = 0x0A, + subcmd_rx_bind = 0x01, + }; + + 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 + } ; + 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); From 85ef245d079789b688d7356fe6b6ef116f94d7a3 Mon Sep 17 00:00:00 2001 From: benjinne Date: Tue, 18 Jun 2024 10:41:50 -0400 Subject: [PATCH 2/5] crsf_rc: bind only if not singlewire and return success ack if uart write is successful --- src/drivers/rc/crsf_rc/CrsfRc.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/drivers/rc/crsf_rc/CrsfRc.cpp b/src/drivers/rc/crsf_rc/CrsfRc.cpp index b42ea714fffe..de079f8d106d 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.cpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.cpp @@ -194,9 +194,11 @@ void CrsfRc::Run() uint8_t cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; - if (!_armed) { - BindCRSF(); - cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + if (!_is_singlewire && !_armed) { + if(BindCRSF()) + { + cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + } } // publish acknowledgement From 39a75b8c45350f94af95716f64933073255d98a9 Mon Sep 17 00:00:00 2001 From: benjinne Date: Tue, 18 Jun 2024 13:50:17 -0400 Subject: [PATCH 3/5] crsf_rc: bind feature fix format --- src/drivers/rc/crsf_rc/CrsfRc.cpp | 25 ++++++++++++------------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/src/drivers/rc/crsf_rc/CrsfRc.cpp b/src/drivers/rc/crsf_rc/CrsfRc.cpp index de079f8d106d..ca6b9fea0afb 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.cpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.cpp @@ -195,8 +195,7 @@ void CrsfRc::Run() uint8_t cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; if (!_is_singlewire && !_armed) { - if(BindCRSF()) - { + if (BindCRSF()) { cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; } } @@ -513,17 +512,17 @@ bool CrsfRc::SendTelemetryFlightMode(const char *flight_mode) bool CrsfRc::BindCRSF() { - uint8_t bindFrame[] = { - (uint8_t)crsf_sync_t::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 - }; + uint8_t bindFrame[] = { + (uint8_t)crsf_sync_t::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 *) bindFrame, 9); } From 33781a0e96907fda10ff049ea51dd3b89ef89143 Mon Sep 17 00:00:00 2001 From: benjinne Date: Tue, 5 Nov 2024 07:50:41 -0500 Subject: [PATCH 4/5] CRSF bind based on rc_type 1 --- src/drivers/rc/crsf_rc/CrsfRc.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/drivers/rc/crsf_rc/CrsfRc.cpp b/src/drivers/rc/crsf_rc/CrsfRc.cpp index ca6b9fea0afb..b68a7149ca29 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.cpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.cpp @@ -195,9 +195,15 @@ void CrsfRc::Run() uint8_t cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; if (!_is_singlewire && !_armed) { - if (BindCRSF()) { - cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + if ((int)vcmd.param1 == 1) { + // 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 From 1336fb2417cda1fcbdb5191ba671d0fce6344bb6 Mon Sep 17 00:00:00 2001 From: benjinne Date: Wed, 27 Nov 2024 12:51:31 -0500 Subject: [PATCH 5/5] rc drivers: fix RC_TYPE magic numbers with vehicleCommand constants --- msg/versioned/VehicleCommand.msg | 7 +++++++ src/drivers/rc/crsf_rc/CrsfRc.cpp | 2 +- src/drivers/rc_input/RCInput.cpp | 8 ++++---- 3 files changed, 12 insertions(+), 5 deletions(-) diff --git a/msg/versioned/VehicleCommand.msg b/msg/versioned/VehicleCommand.msg index 96162e9f89f9..3a0c26bc42d0 100644 --- a/msg/versioned/VehicleCommand.msg +++ b/msg/versioned/VehicleCommand.msg @@ -169,6 +169,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 b68a7149ca29..080bba7c7abe 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.cpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.cpp @@ -195,7 +195,7 @@ void CrsfRc::Run() uint8_t cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; if (!_is_singlewire && !_armed) { - if ((int)vcmd.param1 == 1) { + 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; diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index 12cd1ab0bf40..83eeb99b0b1c 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -374,19 +374,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; }