Cleanup

radio/src/telemetry/telemetry.cpp

@@ -31,6 +31,9 @@
   #include "libopenui.h"
 #endif
 
+#include <FreeRTOS.h>
+#include <timers.h>
+
 uint8_t telemetryStreaming = 0;
 uint8_t telemetryRxBuffer[TELEMETRY_RX_PACKET_SIZE];   // Receive buffer. 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1)
 uint8_t telemetryRxBufferCount = 0;
@@ -106,6 +109,45 @@ void telemetryMirrorSend(uint8_t data)
   }
 }
 
+#if !defined(SIMU)
+static TimerHandle_t telemetryTimer = nullptr;
+static StaticTimer_t telemetryTimerBuffer;
+
+static void telemetryTimerCb(TimerHandle_t xTimer)
+{
+  (void)xTimer;
+  if (!s_pulses_paused) {
+    DEBUG_TIMER_START(debugTimerTelemetryWakeup);
+    telemetryWakeup();
+    DEBUG_TIMER_STOP(debugTimerTelemetryWakeup);
+  }
+}
+
+void telemetryStart()
+{
+  if (!telemetryTimer) {
+    telemetryTimer =
+        xTimerCreateStatic("Telem", 4 / RTOS_MS_PER_TICK, pdTRUE, (void*)0,
+                           telemetryTimerCb, &telemetryTimerBuffer);
+  }
+
+  if (telemetryTimer) {
+    if( xTimerStart( telemetryTimer, 0 ) != pdPASS ) {
+      /* The timer could not be set into the Active state. */
+    }
+  }
+}
+
+void telemetryStop()
+{
+  if (telemetryTimer) {
+    if( xTimerStop( telemetryTimer, 5 / RTOS_MS_PER_TICK ) != pdPASS ) {
+      /* The timer could not be set into the Active state. */
+    }
+  }
+}
+#endif
+
 void processTelemetryData(uint8_t data)
 {
 #if defined(CROSSFIRE)