From 425f2b946ea1dd94ea512627e6e50cacd6adb861 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 22 Nov 2022 20:22:38 +0000 Subject: [PATCH] AP_RCProtocol: check for 3 good frames for CRSF Move get_link_rate() and get_protocol_string() to CRSF protocol allow ELRS at 420kbaud to be configured allow CRSF to bootstrap at ELRS desired baudrate --- libraries/AP_RCProtocol/AP_RCProtocol.h | 2 +- .../AP_RCProtocol/AP_RCProtocol_CRSF.cpp | 51 +++++++++++++++---- libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h | 30 ++++++++--- 3 files changed, 66 insertions(+), 17 deletions(-) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index 58d48bed67..2caf8a1cda 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -101,12 +101,12 @@ public: #if AP_RCPROTOCOL_FPORT2_ENABLED case FPORT2: #endif + case CRSF: return true; case IBUS: case SUMD: case SRXL: case SRXL2: - case CRSF: case ST24: case NONE: return false; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp index 0e5b50a9b8..8671b83212 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp @@ -153,7 +153,11 @@ static const char* get_frame_type(uint8_t byte, uint8_t subtype = 0) #define CRSF_DIGITAL_CHANNEL_MAX 1811 -constexpr uint16_t AP_RCProtocol_CRSF::elrs_air_rates[8]; +const uint16_t AP_RCProtocol_CRSF::RF_MODE_RATES[RFMode::RF_MODE_MAX_MODES] = { + 4, 50, 150, 250, // CRSF + 4, 25, 50, 100, 150, 200, 250, 500 // ELRS +}; + AP_RCProtocol_CRSF* AP_RCProtocol_CRSF::_singleton; AP_RCProtocol_CRSF::AP_RCProtocol_CRSF(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) @@ -181,11 +185,23 @@ AP_RCProtocol_CRSF::~AP_RCProtocol_CRSF() { _singleton = nullptr; } -void AP_RCProtocol_CRSF::process_pulse(uint32_t width_s0, uint32_t width_s1) -{ - uint8_t b; - if (ss.process_pulse(width_s0, width_s1, b)) { - _process_byte(ss.get_byte_timestamp_us(), b); +// get the protocol string +const char* AP_RCProtocol_CRSF::get_protocol_string(ProtocolType protocol) const { + if (protocol == ProtocolType::PROTOCOL_ELRS) { + return "ELRS"; + } else if (_crsf_v3_active) { + return "CRSFv3"; + } else { + return "CRSFv2"; + } +} + +// return the link rate as defined by the LinkStatistics +uint16_t AP_RCProtocol_CRSF::get_link_rate(ProtocolType protocol) const { + if (protocol == ProtocolType::PROTOCOL_ELRS) { + return RF_MODE_RATES[_link_status.rf_mode + RFMode::ELRS_RF_MODE_4HZ]; + } else { + return RF_MODE_RATES[_link_status.rf_mode]; } } @@ -386,7 +402,7 @@ bool AP_RCProtocol_CRSF::decode_crsf_packet() // now wait for 4ms to account for RX transmission and processing hal.scheduler->delay(4); // change the baud rate - uart->begin(_new_baud_rate, 128, 128); + uart->begin(_new_baud_rate); } _new_baud_rate = 0; } @@ -559,7 +575,7 @@ void AP_RCProtocol_CRSF::start_uart() _uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); _uart->set_blocking_writes(false); _uart->set_options(_uart->get_options() & ~AP_HAL::UARTDriver::OPTION_RXINV); - _uart->begin(CRSF_BAUDRATE, 128, 128); + _uart->begin(get_bootstrap_baud_rate()); } // change the baudrate of the protocol if we are able @@ -570,7 +586,7 @@ bool AP_RCProtocol_CRSF::change_baud_rate(uint32_t baudrate) return false; } #if !defined(STM32H7) - if (baudrate > CRSF_BAUDRATE && !uart->is_dma_enabled()) { + if (baudrate > get_bootstrap_baud_rate() && !uart->is_dma_enabled()) { return false; } #endif @@ -583,6 +599,23 @@ bool AP_RCProtocol_CRSF::change_baud_rate(uint32_t baudrate) return true; } +// change the bootstrap baud rate to ELRS standard if configured +void AP_RCProtocol_CRSF::process_handshake(uint32_t baudrate) +{ + AP_HAL::UARTDriver *uart = get_current_UART(); + + // only change the baudrate if we are bootstrapping CRSF + if (uart == nullptr + || baudrate != CRSF_BAUDRATE + || baudrate == get_bootstrap_baud_rate() + || uart->get_baud_rate() == get_bootstrap_baud_rate() + || (get_rc_protocols_mask() & ((1U<<(uint8_t(AP_RCProtocol::CRSF)+1))+1)) == 0) { + return; + } + + uart->begin(get_bootstrap_baud_rate()); +} + //returns uplink link quality on 0-255 scale int16_t AP_RCProtocol_CRSF::derive_scaled_lq_value(uint8_t uplink_lq) { diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h index 3e98886add..7562d594b5 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h @@ -20,6 +20,7 @@ #include "AP_RCProtocol.h" #include +#include #include "SoftSerial.h" #define CRSF_MAX_CHANNELS 24U // Maximum number of channels from crsf datastream @@ -27,6 +28,7 @@ #define CSRF_HEADER_LEN 2U // header length #define CRSF_FRAME_PAYLOAD_MAX (CRSF_FRAMELEN_MAX - CSRF_HEADER_LEN) // maximum size of the frame length field in a packet #define CRSF_BAUDRATE 416666U +#define ELRS_BAUDRATE 420000U #define CRSF_TX_TIMEOUT 500000U // the period after which the transmitter is considered disconnected (matches copters failsafe) #define CRSF_RX_TIMEOUT 150000U // the period after which the receiver is considered disconnected (>ping frequency) @@ -35,11 +37,14 @@ public: AP_RCProtocol_CRSF(AP_RCProtocol &_frontend); virtual ~AP_RCProtocol_CRSF(); void process_byte(uint8_t byte, uint32_t baudrate) override; - void process_pulse(uint32_t width_s0, uint32_t width_s1) override; + void process_handshake(uint32_t baudrate) override; void update(void) override; // support for CRSF v3 bool change_baud_rate(uint32_t baudrate); - bool is_crsf_v3_active() const { return _crsf_v3_active; } + // bootstrap baudrate + uint32_t get_bootstrap_baud_rate() const { + return rc().use_420kbaud_for_elrs() ? ELRS_BAUDRATE : CRSF_BAUDRATE; + } // is the receiver active, used to detect power loss and baudrate changes bool is_rx_active() const override { @@ -233,7 +238,13 @@ public: // uint16_t digital_switch_channel[]:10; // digital switch channel } PACKED; - enum class RFMode : uint8_t { + enum class ProtocolType { + PROTOCOL_CRSF, + PROTOCOL_TRACER, + PROTOCOL_ELRS + }; + + enum RFMode { CRSF_RF_MODE_4HZ = 0, CRSF_RF_MODE_50HZ, CRSF_RF_MODE_150HZ, @@ -246,10 +257,9 @@ public: ELRS_RF_MODE_200HZ, ELRS_RF_MODE_250HZ, ELRS_RF_MODE_500HZ, + RF_MODE_MAX_MODES, RF_MODE_UNKNOWN, }; - // nominal ELRS air rates - static constexpr uint16_t elrs_air_rates[8] = {4, 25, 50, 100, 150, 200, 250, 500}; struct LinkStatus { int16_t rssi = -1; @@ -263,6 +273,12 @@ public: return _link_status; } + // return the link rate as defined by the LinkStatistics + uint16_t get_link_rate(ProtocolType protocol) const; + + // return the protocol string + const char* get_protocol_string(ProtocolType protocol) const; + private: struct Frame _frame; struct Frame _telemetry_frame; @@ -304,9 +320,9 @@ private: volatile struct LinkStatus _link_status; - AP_HAL::UARTDriver *_uart; + static const uint16_t RF_MODE_RATES[RFMode::RF_MODE_MAX_MODES]; - SoftSerial ss{CRSF_BAUDRATE, SoftSerial::SERIAL_CONFIG_8N1}; + AP_HAL::UARTDriver *_uart; }; namespace AP {