diff --git a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp index 4f76626701..a27a38ce1f 100644 --- a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp +++ b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp @@ -66,6 +66,11 @@ bool AP_CRSF_Telem::init(void) return false; } + // Someone explicitly configure CRSF control for VTX + if (AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_CRSF, 0)) { + AP::vtx().set_provider_enabled(AP_VideoTX::VTXType::CRSF); + } + return AP_RCTelemetry::init(); } @@ -569,6 +574,8 @@ void AP_CRSF_Telem::process_vtx_frame(VTXFrame* vtx) { return; } + apvtx.set_provider_enabled(AP_VideoTX::VTXType::CRSF); + apvtx.set_band(vtx->band); apvtx.set_channel(vtx->channel); if (vtx->is_in_user_frequency_mode) { @@ -615,6 +622,8 @@ void AP_CRSF_Telem::process_vtx_telem_frame(VTXTelemetryFrame* vtx) return; } + apvtx.set_provider_enabled(AP_VideoTX::VTXType::CRSF); + apvtx.set_frequency_mhz(vtx->frequency); AP_VideoTX::VideoBand band; @@ -746,11 +755,6 @@ void AP_CRSF_Telem::process_param_read_frame(ParameterSettingsReadFrame* read_fr _pending_request.frame_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_READ; } -// process any changed settings and schedule for transmission -void AP_CRSF_Telem::update() -{ -} - void AP_CRSF_Telem::process_pending_requests() { // handle general parameter requests @@ -781,7 +785,8 @@ void AP_CRSF_Telem::update_vtx_params() { AP_VideoTX& vtx = AP::vtx(); - if (!vtx.get_enabled()) { + // This function does ugly things with the vtx parameters which will upset other providers + if (!vtx.get_enabled() || !vtx.is_provider_enabled(AP_VideoTX::VTXType::CRSF)) { return; } diff --git a/libraries/AP_RCTelemetry/AP_CRSF_Telem.h b/libraries/AP_RCTelemetry/AP_CRSF_Telem.h index f02e531a57..dcebf23d0c 100644 --- a/libraries/AP_RCTelemetry/AP_CRSF_Telem.h +++ b/libraries/AP_RCTelemetry/AP_CRSF_Telem.h @@ -243,8 +243,6 @@ public: // Process a frame from the CRSF protocol decoder static bool process_frame(AP_RCProtocol_CRSF::FrameType frame_type, void* data); - // process any changed settings and schedule for transmission - void update(); // get next telemetry data for external consumers of SPort data static bool get_telem_data(AP_RCProtocol_CRSF::Frame* frame, bool is_tx_active); // start bind request