diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index d2d11cfb304bf..eba84a6343c08 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -147,6 +147,7 @@ void AP_RCProtocol::process_pulse(uint32_t width_s0, uint32_t width_s1) // this protocol is disabled for pulse input continue; } + if (backend[i] != nullptr) { if (!protocol_enabled(rcprotocol_t(i))) { continue; @@ -197,6 +198,33 @@ void AP_RCProtocol::process_pulse_list(const uint32_t *widths, uint16_t n, bool } } +bool AP_RCProtocol::process_frame(const uint8_t* buf, ssize_t nbytes) +{ + uint32_t now = AP_HAL::millis(); + bool searching = should_search(now); + +#if AP_RC_CHANNEL_ENABLED + rc_protocols_mask = rc().enabled_protocols(); +#endif + + if (_detected_protocol != AP_RCProtocol::NONE && + !protocol_enabled(_detected_protocol)) { + _detected_protocol = AP_RCProtocol::NONE; + } + + // first try current protocol + if (_detected_protocol != AP_RCProtocol::NONE && !searching) { + backend[_detected_protocol]->process_frame(buf, nbytes); + if (backend[_detected_protocol]->new_input()) { + _new_input = true; + _last_input_ms = now; + } + return true; + } + + return false; +} + bool AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate) { uint32_t now = AP_HAL::millis(); @@ -226,12 +254,14 @@ bool AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate) return true; } + // otherwise scan all protocols for (uint8_t i = 0; i < ARRAY_SIZE(backend); i++) { if (backend[i] != nullptr) { if (!protocol_enabled(rcprotocol_t(i))) { continue; } + const uint32_t frame_count = backend[i]->get_rc_frame_count(); const uint32_t input_count = backend[i]->get_rc_input_count(); backend[i]->process_byte(byte, baudrate); @@ -309,7 +339,7 @@ static const AP_RCProtocol::SerialConfig serial_configs[] { #endif }; -static_assert(ARRAY_SIZE(serial_configs) > 1, "must have at least one serial config"); +static_assert(ARRAY_SIZE(serial_configs) > 0, "must have at least one serial config"); void AP_RCProtocol::check_added_uart(void) { @@ -333,14 +363,32 @@ void AP_RCProtocol::check_added_uart(void) const uint32_t current_baud = serial_configs[added.config_num].baud; process_handshake(current_baud); - uint32_t n = added.uart->available(); - n = MIN(n, 255U); - for (uint8_t i=0; iread(); - if (b >= 0) { - process_byte(uint8_t(b), current_baud); + // processing in frame-based mode + if (!searching && backend[_detected_protocol]->frame_input_enabled()) { + uint8_t buf[PROTOCOL_MAX_FRAME_SIZE]; + while (added.uart->frame_available()) { + ssize_t nbytes = added.uart->read_frame(buf, PROTOCOL_MAX_FRAME_SIZE); + process_frame(buf, nbytes); + } + } else { // processing in byte-based mode + if (_detected_protocol != AP_RCProtocol::NONE + && backend[_detected_protocol]->frame_input_enabled()) { + // searching again, make sure we are in byte mode + backend[_detected_protocol]->frame_input_enabled(added.uart, false); + } + + uint32_t n = added.uart->available(); + n = MIN(n, 255U); + for (uint8_t i=0; iread(); + if (b >= 0) { + if (process_byte(uint8_t(b), current_baud)) { + backend[_detected_protocol]->frame_input_enabled(added.uart, true); + } + } } } + if (searching) { if (now - added.last_config_change_ms > 1000) { // change configs if not detected once a second diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index 8889fdce89ee4..4ac741cf896fa 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -94,6 +94,7 @@ class AP_RCProtocol { void process_pulse(uint32_t width_s0, uint32_t width_s1); void process_pulse_list(const uint32_t *widths, uint16_t n, bool need_swap); bool process_byte(uint8_t byte, uint32_t baudrate); + bool process_frame(const uint8_t* buf, ssize_t nbytes); void process_handshake(uint32_t baudrate); void update(void); diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index e0a128dc30a3c..69bcd76c27b88 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -99,6 +99,11 @@ class AP_RCProtocol_Backend { return frontend._detected_protocol != AP_RCProtocol::NONE && frontend.backend[frontend._detected_protocol] == this; } + // framing API + virtual void frame_input_enabled(AP_HAL::UARTDriver* uart, bool onoff) { _framing_enabled = onoff; } + bool frame_input_enabled() const { return _framing_enabled; } + virtual void process_frame(const uint8_t* buffer, uint16_t buflen) { } + #if AP_VIDEOTX_ENABLED // called by static methods to confiig video transmitters: static void configure_vtx(uint8_t band, uint8_t channel, uint8_t power, uint8_t pitmode); @@ -132,6 +137,7 @@ class AP_RCProtocol_Backend { uint32_t rc_input_count; uint32_t last_rc_input_count; uint32_t rc_frame_count; + bool _framing_enabled; uint16_t _pwm_values[MAX_RCIN_CHANNELS]; uint8_t _num_channels; @@ -139,4 +145,6 @@ class AP_RCProtocol_Backend { int16_t rx_link_quality = -1; }; +#define PROTOCOL_MAX_FRAME_SIZE 64 + #endif // AP_RCPROTOCOL_ENABLED diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp index b9d68b63900b5..ed6817dc69f27 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp @@ -231,6 +231,16 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte) _start_frame_time_us = timestamp_us; } + _process_raw_byte(timestamp_us, byte); +} + +void AP_RCProtocol_CRSF::_process_raw_byte(uint32_t timestamp_us, uint8_t byte) +{ + // overflow check + if (_frame_ofs >= CRSF_FRAMELEN_MAX) { + _frame_ofs = 0; + } + add_to_buffer(_frame_ofs++, byte); // need a header to get the length @@ -238,6 +248,10 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte) return; } + if (_frame.device_address != DeviceAddress::CRSF_ADDRESS_FLIGHT_CONTROLLER) { + return; + } + // parse the length if (_frame_ofs == CSRF_HEADER_TYPE_LEN) { _frame_crc = crc8_dvb_s2(0, _frame.type); @@ -280,6 +294,34 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte) } } +void AP_RCProtocol_CRSF::frame_input_enabled(AP_HAL::UARTDriver* uart, bool onoff) +{ + // nothing to do + if (onoff == AP_RCProtocol_Backend::frame_input_enabled()) { + return; + } + + // due to bugs in CRSFv3, framing is not possible at higher baudrates + if (onoff && uart->get_baud_rate() > CRSF_BAUDRATE) { + return; + } + + AP_RCProtocol_Backend::frame_input_enabled(uart, onoff); + if (onoff) { + uart->begin_framing(CRSF_FRAMELEN_MAX); + } else { + uart->end_framing(); + } +} + +void AP_RCProtocol_CRSF::process_frame(const uint8_t* buffer, uint16_t buflen) +{ + uint32_t now_us = AP_HAL::micros(); + for (uint8_t i = 0; i < buflen; i++) { + _process_raw_byte(now_us, buffer[i]); + } +} + void AP_RCProtocol_CRSF::update(void) { // if we are in standalone mode, process data from the uart @@ -412,6 +454,9 @@ bool AP_RCProtocol_CRSF::decode_crsf_packet() hal.scheduler->delay(4); // change the baud rate uart->begin(_new_baud_rate); + if (!AP_RCProtocol_Backend::frame_input_enabled()) { + uart->begin_framing(CRSF_FRAMELEN_MAX); + } } _new_baud_rate = 0; } @@ -598,6 +643,11 @@ bool AP_RCProtocol_CRSF::change_baud_rate(uint32_t baudrate) return false; } #endif + + if (AP_RCProtocol_Backend::frame_input_enabled()) { + return false; + } + if (baudrate > CRSF_BAUDRATE_2MBIT) { return false; } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h index c063175e01f3e..6693b7ef152e5 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h @@ -54,6 +54,9 @@ class AP_RCProtocol_CRSF : public AP_RCProtocol_Backend { #endif } + void frame_input_enabled(AP_HAL::UARTDriver* uart, bool onoff) override; + void process_frame(const uint8_t* buffer, uint16_t buflen) override; + // is the receiver active, used to detect power loss and baudrate changes bool is_rx_active() const override { // later versions of CRSFv3 will send link rate frames every 200ms @@ -298,6 +301,7 @@ class AP_RCProtocol_CRSF : public AP_RCProtocol_Backend { static AP_RCProtocol_CRSF* _singleton; void _process_byte(uint32_t timestamp_us, uint8_t byte); + void _process_raw_byte(uint32_t timestamp_us, uint8_t byte); bool decode_crsf_packet(); bool process_telemetry(bool check_constraint = true); void process_link_stats_frame(const void* data); diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp index 9a8a68f871383..ea6295049fbc9 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp @@ -152,6 +152,10 @@ void AP_RCProtocol_GHST::_process_byte(uint32_t timestamp_us, uint8_t byte) return; } + if (_frame.device_address != DeviceAddress::GHST_ADDRESS_FLIGHT_CONTROLLER) { + return; + } + // parse the length if (_frame_ofs == GHST_HEADER_TYPE_LEN) { _frame_crc = crc8_dvb_s2(0, _frame.type); @@ -421,17 +425,17 @@ void AP_RCProtocol_GHST::process_byte(uint8_t byte, uint32_t baudrate) _process_byte(AP_HAL::micros(), byte); } -// change the bootstrap baud rate to ELRS standard if configured +// change the bootstrap baud rate to Ghost standard if configured void AP_RCProtocol_GHST::process_handshake(uint32_t baudrate) { AP_HAL::UARTDriver *uart = get_current_UART(); - // only change the baudrate if we are bootstrapping CRSF + // only change the baudrate if we are specifically bootstrapping Ghost if (uart == nullptr || baudrate != CRSF_BAUDRATE || baudrate == GHST_BAUDRATE || uart->get_baud_rate() == GHST_BAUDRATE - || (get_rc_protocols_mask() & ((1U<<(uint8_t(AP_RCProtocol::GHST)+1))+1)) == 0) { + || (get_rc_protocols_mask() & (1U<<(uint8_t(AP_RCProtocol::GHST)+1))) == 0) { return; }