diff --git a/radio/src/telemetry/telemetry.cpp b/radio/src/telemetry/telemetry.cpp index 9d62b87a9..96ca21100 100644 --- a/radio/src/telemetry/telemetry.cpp +++ b/radio/src/telemetry/telemetry.cpp @@ -265,30 +265,33 @@ void telemetryWakeup() if (int_drv) pollTelemetry(INTERNAL_MODULE, int_drv, getIntModuleCtx()); #endif + bool disable_legacy_polling = false; #if defined(HARDWARE_EXTERNAL_MODULE) auto ext_drv = getExtModuleDriver(); if (ext_drv) { if (pollTelemetry(EXTERNAL_MODULE, ext_drv, getExtModuleCtx())) { // skip legacy telemetry polling in case external module // driver defines telemetry methods (getByte & processData) - return; + disable_legacy_polling = true; } } #endif - - // TODO: needs to be moved to protocol/module init - // as-is, it implies only ONE telemetry protocol - // enabled at the same time - uint8_t requiredTelemetryProtocol = modelTelemetryProtocol(); - if (telemetryProtocol != requiredTelemetryProtocol) { - telemetryInit(requiredTelemetryProtocol); + if (!disable_legacy_polling) { + // TODO: needs to be moved to protocol/module init + // as-is, it implies only ONE telemetry protocol + // enabled at the same time + uint8_t requiredTelemetryProtocol = modelTelemetryProtocol(); + + if (telemetryProtocol != requiredTelemetryProtocol) { + telemetryInit(requiredTelemetryProtocol); + } + + // Poll external / S.PORT telemetry + // TODO: how to switch this OFF ??? + pollExtTelemetryLegacy(); } - // Poll external / S.PORT telemetry - // TODO: how to switch this OFF ??? - pollExtTelemetryLegacy(); - for (int i=0; i