From 324c8e9eeffaaa76ff9798271b5915969a156dbe Mon Sep 17 00:00:00 2001
From: Raphael Coeffic <raphaelcoeffic@users.noreply.github.com>
Date: Tue, 11 Oct 2022 13:36:24 +0200
Subject: [PATCH] fix(PXX2): prevent legacy telemetry polling (S.PORT) if the
 module driver defines its own method (#2504)

This is to prevent the S.PORT line to interfere with PXX2 telemetry reception, especially with R9M ACCESS.
---
 radio/src/pulses/afhds3.cpp       |  2 ++
 radio/src/pulses/pxx2.cpp         |  3 +++
 radio/src/telemetry/telemetry.cpp | 14 +++++++++++---
 3 files changed, 16 insertions(+), 3 deletions(-)

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(&params));
@@ -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