AP_RCTelemetry: only update VTX parameters if CRSF provider was enabled

This commit is contained in:
Andy Piper 2024-04-29 19:09:43 +01:00 committed by Randy Mackay
parent 8a0066db57
commit 51783d95dd
2 changed files with 11 additions and 8 deletions

View File

@ -66,6 +66,11 @@ bool AP_CRSF_Telem::init(void)
return false; 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(); return AP_RCTelemetry::init();
} }
@ -550,6 +555,8 @@ void AP_CRSF_Telem::process_vtx_frame(VTXFrame* vtx) {
return; return;
} }
apvtx.set_provider_enabled(AP_VideoTX::VTXType::CRSF);
apvtx.set_band(vtx->band); apvtx.set_band(vtx->band);
apvtx.set_channel(vtx->channel); apvtx.set_channel(vtx->channel);
if (vtx->is_in_user_frequency_mode) { if (vtx->is_in_user_frequency_mode) {
@ -596,6 +603,8 @@ void AP_CRSF_Telem::process_vtx_telem_frame(VTXTelemetryFrame* vtx)
return; return;
} }
apvtx.set_provider_enabled(AP_VideoTX::VTXType::CRSF);
apvtx.set_frequency_mhz(vtx->frequency); apvtx.set_frequency_mhz(vtx->frequency);
AP_VideoTX::VideoBand band; AP_VideoTX::VideoBand band;
@ -727,11 +736,6 @@ void AP_CRSF_Telem::process_param_read_frame(ParameterSettingsReadFrame* read_fr
_pending_request.frame_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_READ; _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() void AP_CRSF_Telem::process_pending_requests()
{ {
// handle general parameter requests // handle general parameter requests
@ -762,7 +766,8 @@ void AP_CRSF_Telem::update_vtx_params()
{ {
AP_VideoTX& vtx = AP::vtx(); 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; return;
} }

View File

@ -232,8 +232,6 @@ public:
// Process a frame from the CRSF protocol decoder // Process a frame from the CRSF protocol decoder
static bool process_frame(AP_RCProtocol_CRSF::FrameType frame_type, void* data); 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 // get next telemetry data for external consumers of SPort data
static bool get_telem_data(AP_RCProtocol_CRSF::Frame* frame, bool is_tx_active); static bool get_telem_data(AP_RCProtocol_CRSF::Frame* frame, bool is_tx_active);