From 8f85097c907a7b59d8638981a1c2836cdf245d02 Mon Sep 17 00:00:00 2001
From: Raphael Coeffic <raphael.coeffic@frafos.com>
Date: Sun, 3 Jul 2022 07:18:08 +0200
Subject: [PATCH] MPM proto scan: use RTOS timers instead of polling for
 timeout

---
 radio/src/io/multi_protolist.cpp  | 58 +++++++++++++++++++++++++++++++
 radio/src/io/multi_protolist.h    | 10 ++++++
 radio/src/telemetry/telemetry.cpp | 11 ------
 3 files changed, 68 insertions(+), 11 deletions(-)

diff --git a/radio/src/io/multi_protolist.cpp b/radio/src/io/multi_protolist.cpp
index 1f7749bcc..85181cd75 100644
--- a/radio/src/io/multi_protolist.cpp
+++ b/radio/src/io/multi_protolist.cpp
@@ -32,6 +32,28 @@
 
 #include <algorithm>
 
+#if !defined(SIMU)
+
+#include <FreeRTOS/include/FreeRTOS.h>
+#include <FreeRTOS/include/timers.h>
+
+struct ProtoScanTimer {
+  TimerHandle_t timer = nullptr;
+  StaticTimer_t timerBuffer;
+};
+
+static ProtoScanTimer _protoScanTimers[NUM_MODULES];
+
+void MultiRfProtocols::timerCb(TimerHandle_t xTimer)
+{
+  uint8_t moduleIdx = (uint8_t)(uintptr_t)pvTimerGetTimerID(xTimer);
+  auto instance = MultiRfProtocols::instance(moduleIdx);
+  if (instance->scanState == ScanBegin || instance->scanState == Scanning) {
+    instance->fillBuiltinProtos();
+  }
+}
+#endif
+
 MultiRfProtocols* MultiRfProtocols::_instance[NUM_MODULES] = {};
 
 // MPM telemetry packet format type = MultiProtoDef (0x11)
@@ -231,6 +253,22 @@ bool MultiRfProtocols::triggerScan()
     currentProto = MULTI_INVALID_PROTO;
     moduleState[moduleIdx].mode = MODULE_MODE_GET_HARDWARE_INFO;
     scanStart = lastScan = RTOS_GET_MS();
+
+#if !defined(SIMU)
+    auto scanTimer = &_protoScanTimers[moduleIdx];
+    if (!scanTimer->timer) {
+      scanTimer->timer = xTimerCreateStatic(
+          "MPM", MULTI_PROTOLIST_START_TIMEOUT / RTOS_MS_PER_TICK, pdTRUE,
+          (void*)moduleIdx, MultiRfProtocols::timerCb, &scanTimer->timerBuffer);
+    }
+
+    if (scanTimer->timer) {
+      if( xTimerStart( scanTimer->timer, 0 ) != pdPASS ) {
+        /* The timer could not be set into the Active state. */
+      }
+    }
+#endif
+    
     return true;
   }
 
@@ -290,11 +328,31 @@ bool MultiRfProtocols::scanReply(const uint8_t* packet, uint8_t len)
 
           currentProto++;
           lastScan = RTOS_GET_MS();
+
+#if !defined(SIMU)
+          auto scanTimer = &_protoScanTimers[moduleIdx];
+          if (scanTimer->timer) {
+            if (xTimerChangePeriod(scanTimer->timer,
+                                   MULTI_PROTOLIST_TIMEOUT / RTOS_MS_PER_TICK,
+                                   0) != pdPASS) {
+              /* The timer period could not be reset. */
+            }
+          }
+#endif
           return true;
 
         } else {
           scanState = ScanEnd;
           setModuleMode(moduleIdx, MODULE_MODE_NORMAL);
+
+#if !defined(SIMU)
+          auto scanTimer = &_protoScanTimers[moduleIdx];
+          if (scanTimer->timer) {
+            if (xTimerStop(scanTimer->timer, 0) != pdPASS) {
+              /* The timer period could not be stopped. */
+            }
+          }
+#endif
           break;
         }
       } else {
diff --git a/radio/src/io/multi_protolist.h b/radio/src/io/multi_protolist.h
index b14e9e1bd..b1d4d92a0 100644
--- a/radio/src/io/multi_protolist.h
+++ b/radio/src/io/multi_protolist.h
@@ -27,6 +27,12 @@
 #include <vector>
 #include <map>
 
+#if !defined(SIMU)
+// Forward declare FreeRTOS timer
+struct tmrTimerControl;
+typedef struct tmrTimerControl * TimerHandle_t;
+#endif
+
 class MultiRfProtocols
 {
   static MultiRfProtocols* _instance[NUM_MODULES];
@@ -45,6 +51,10 @@ class MultiRfProtocols
   MultiRfProtocols(unsigned int moduleIdx);
   void fillBuiltinProtos();
 
+#if !defined(SIMU)
+  static void timerCb(TimerHandle_t xTimer);
+#endif
+  
  public:
 
   struct RfProto {
diff --git a/radio/src/telemetry/telemetry.cpp b/radio/src/telemetry/telemetry.cpp
index c18b28ee4..075671d0e 100644
--- a/radio/src/telemetry/telemetry.cpp
+++ b/radio/src/telemetry/telemetry.cpp
@@ -223,17 +223,6 @@ inline bool isBadAntennaDetected()
   return false;
 }
 
-#if defined(MULTI_PROTOLIST)
-// TODO: FreeRTOS timer instead...
-static inline void pollMultiProtolist(uint8_t idx)
-{
-  if ((moduleState[idx].protocol == PROTOCOL_CHANNELS_MULTIMODULE) &&
-      MultiRfProtocols::instance(idx)->isScanning()) {
-    MultiRfProtocols::instance(idx)->scanReply();
-  }
-}
-#endif
-
 static inline void pollTelemetry(uint8_t module, const etx_module_driver_t* drv, void* ctx)
 {
   if (!drv || !drv->getByte || !drv->processData) return;