diff --git a/radio/src/pulses/afhds3.cpp b/radio/src/pulses/afhds3.cpp index 65ddff7302b..701a0311e1c 100644 --- a/radio/src/pulses/afhds3.cpp +++ b/radio/src/pulses/afhds3.cpp @@ -924,6 +924,7 @@ static void* initInternal(uint8_t module) auto p_state = &protoState[module]; p_state->init(module, &intmodulePulsesData, &IntmoduleSerialDriver); + telemetryProtocol = PROTOCOL_TELEMETRY_AFHDS3; mixerSchedulerSetPeriod(module, AFHDS3_UART_COMMAND_TIMEOUT * 1000 /* us */); INTERNAL_MODULE_ON(); @@ -933,6 +934,7 @@ static void* initInternal(uint8_t module) static void deinitInternal(void* context) { INTERNAL_MODULE_OFF(); + telemetryProtocol = 0xFF; mixerSchedulerSetPeriod(INTERNAL_MODULE, 0); auto p_state = (ProtoState*)context; diff --git a/radio/src/pulses/pxx2.cpp b/radio/src/pulses/pxx2.cpp index da774f4f2de..91e9c816cb1 100644 --- a/radio/src/pulses/pxx2.cpp +++ b/radio/src/pulses/pxx2.cpp @@ -834,6 +834,8 @@ static void* pxx2InitExternal(uint8_t module, uint32_t baudrate) mixerSchedulerSetPeriod(module, PXX2_NO_HEARTBEAT_PERIOD); EXTERNAL_MODULE_ON(); + telemetryProtocol = PROTOCOL_TELEMETRY_FRSKY_SPORT; + auto state = &pxx2State[module]; state->init(module, &extmodulePulsesData.pxx2, &ExtmoduleSerialDriver, ExtmoduleSerialDriver.init(¶ms)); @@ -856,6 +858,7 @@ static void pxx2DeInitExternal(void* context) EXTERNAL_MODULE_OFF(); mixerSchedulerSetPeriod(EXTERNAL_MODULE, 0); ExtmoduleSerialDriver.deinit(context); + telemetryProtocol = 0xFF; } static void pxx2SetupPulsesExternal(void* context, int16_t* channels, uint8_t nChannels) diff --git a/radio/src/telemetry/telemetry.cpp b/radio/src/telemetry/telemetry.cpp index 691f5fc5b3e..9baec00849a 100644 --- a/radio/src/telemetry/telemetry.cpp +++ b/radio/src/telemetry/telemetry.cpp @@ -223,9 +223,9 @@ inline bool isBadAntennaDetected() return false; } -static inline void pollTelemetry(uint8_t module, const etx_module_driver_t* drv, void* ctx) +static inline bool pollTelemetry(uint8_t module, const etx_module_driver_t* drv, void* ctx) { - if (!drv || !drv->getByte || !drv->processData) return; + if (!drv || !drv->getByte || !drv->processData) return false; uint8_t* rxBuffer = getTelemetryRxBuffer(module); uint8_t& rxBufferCount = getTelemetryRxBufferCount(module); @@ -239,6 +239,8 @@ static inline void pollTelemetry(uint8_t module, const etx_module_driver_t* drv, LOG_TELEMETRY_WRITE_BYTE(data); } while (drv->getByte(ctx, &data) > 0); } + + return true; } static inline void pollExtTelemetryLegacy() @@ -265,7 +267,13 @@ void telemetryWakeup() #if defined(HARDWARE_EXTERNAL_MODULE) auto ext_drv = getExtModuleDriver(); - if (ext_drv) pollTelemetry(EXTERNAL_MODULE, ext_drv, getExtModuleCtx()); + 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; + } + } #endif // TODO: needs to be moved to protocol/module init