diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index e0a128dc30a3c..18aeb33cff90b 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -67,6 +67,10 @@ class AP_RCProtocol_Backend { return frontend.rc_protocols_mask; } + bool protocol_enabled(enum AP_RCProtocol::rcprotocol_t protocol) const { + return frontend.protocol_enabled(protocol); + } + // get RSSI int16_t get_RSSI(void) const { return rssi; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp index b9d68b63900b5..2fd5a35a546c7 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp @@ -238,6 +238,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); @@ -617,7 +621,7 @@ void AP_RCProtocol_CRSF::process_handshake(uint32_t baudrate) || 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) { + || !protocol_enabled(AP_RCProtocol::CRSF)) { return; } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp index 9a8a68f871383..a51c9dd795c88 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,18 @@ 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) { + || !protocol_enabled(AP_RCProtocol::GHST) + || protocol_enabled(AP_RCProtocol::CRSF)) { return; }