From 15047ddfc7655a037bda6928255d696206c4be56 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 1 Jul 2023 14:58:32 +0100 Subject: [PATCH] AP_RCProtocol: rescan at CRSFv3 baud rates to avoid RX loss on soft reboot --- libraries/AP_RCProtocol/AP_RCProtocol.cpp | 4 ++++ libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp | 6 ++++-- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index 4fead7fed5..4b02171695 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -293,8 +293,12 @@ static const AP_RCProtocol::SerialConfig serial_configs[] { // FastSBUS: { 200000, 2, 2, true }, #endif +#if AP_RCPROTOCOL_CRSF_ENABLED // CrossFire: { 416666, 0, 1, false }, + // CRSFv3 can negotiate higher rates which are sticky on soft reboot + { 2000000, 0, 1, false }, +#endif }; static_assert(ARRAY_SIZE(serial_configs) > 1, "must have at least one serial config"); diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp index a78df83f5d..df47ab768d 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp @@ -155,6 +155,8 @@ static const char* get_frame_type(uint8_t byte, uint8_t subtype = 0) #define CRSF_DIGITAL_CHANNEL_MIN 172 #define CRSF_DIGITAL_CHANNEL_MAX 1811 +#define CRSF_BAUDRATE_1MBIT 1000000U +#define CRSF_BAUDRATE_2MBIT 2000000U const uint16_t AP_RCProtocol_CRSF::RF_MODE_RATES[RFMode::RF_MODE_MAX_MODES] = { 4, 50, 150, 250, // CRSF @@ -568,7 +570,7 @@ void AP_RCProtocol_CRSF::process_link_stats_tx_frame(const void* data) void AP_RCProtocol_CRSF::process_byte(uint8_t byte, uint32_t baudrate) { // reject RC data if we have been configured for standalone mode - if (baudrate != CRSF_BAUDRATE || _uart) { + if ((baudrate != CRSF_BAUDRATE && baudrate != CRSF_BAUDRATE_1MBIT && baudrate != CRSF_BAUDRATE_2MBIT) || _uart) { return; } _process_byte(AP_HAL::micros(), byte); @@ -596,7 +598,7 @@ bool AP_RCProtocol_CRSF::change_baud_rate(uint32_t baudrate) return false; } #endif - if (baudrate > 2000000) { + if (baudrate > CRSF_BAUDRATE_2MBIT) { return false; }