From b61e2d8dda539f6449c9fd7ea4876a0e6aeec9f9 Mon Sep 17 00:00:00 2001
From: Raphael Coeffic <raphaelcoeffic@users.noreply.github.com>
Date: Mon, 31 Oct 2022 04:18:39 +0100
Subject: [PATCH] fix(telemetry): sensor processing with legacy polling
 disabled (#2653)

Fixes #2546
Fixes #2591
---
 radio/src/telemetry/telemetry.cpp | 27 +++++++++++++++------------
 1 file changed, 15 insertions(+), 12 deletions(-)

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<MAX_TELEMETRY_SENSORS; i++) {
     const TelemetrySensor & sensor = g_model.telemetrySensors[i];
     if (sensor.type == TELEM_TYPE_CALCULATED) {