diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index cbb0b1135b..e0a128dc30 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -95,6 +95,10 @@ public: return true; } + bool is_detected() const { + return frontend._detected_protocol != AP_RCProtocol::NONE && frontend.backend[frontend._detected_protocol] == this; + } + #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); diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp index df47ab768d..b9d68b6390 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp @@ -302,7 +302,7 @@ void AP_RCProtocol_CRSF::update(void) // never received RC frames, but have received CRSF frames so make sure we give the telemetry opportunity to run uint32_t now = AP_HAL::micros(); - if (_last_frame_time_us > 0 && (!get_rc_frame_count() || !is_tx_active()) + if (_last_frame_time_us > 0 && (!get_rc_input_count() || !is_tx_active()) && now - _last_frame_time_us > CRSF_INTER_FRAME_TIME_US_250HZ) { process_telemetry(false); _last_frame_time_us = now;