From 59191f37c3b7bc52b48356f38b24f912e388ba73 Mon Sep 17 00:00:00 2001
From: Raphael Coeffic <raphael.coeffic@frafos.com>
Date: Fri, 25 Mar 2022 11:27:39 +0100
Subject: [PATCH] Use software timers to trigger telemetry processing

---
 radio/src/FreeRTOSConfig.h  |  5 ++--
 radio/src/io/frsky_pxx2.h   |  3 +--
 radio/src/pulses/pulses.cpp | 51 +++++++++++++++++++++++++++++++++++++
 radio/src/pulses/pulses.h   | 25 ++----------------
 radio/src/tasks.cpp         |  6 -----
 5 files changed, 57 insertions(+), 33 deletions(-)

diff --git a/radio/src/FreeRTOSConfig.h b/radio/src/FreeRTOSConfig.h
index e63e03594..b2f754f88 100644
--- a/radio/src/FreeRTOSConfig.h
+++ b/radio/src/FreeRTOSConfig.h
@@ -42,7 +42,7 @@
 #define configUSE_APPLICATION_TASK_TAG  0
 #define configUSE_COUNTING_SEMAPHORES   0
 #define configGENERATE_RUN_TIME_STATS   0
-#define configUSE_TIMERS                0
+#define configUSE_TIMERS                1
 
 #if !defined(DEBUG)
   #define configMAX_TASK_NAME_LEN         4
@@ -65,7 +65,7 @@
 /* Software timer definitions. */
 #define configTIMER_TASK_PRIORITY       ( 2 )
 #define configTIMER_QUEUE_LENGTH        10
-#define configTIMER_TASK_STACK_DEPTH    ( configMINIMAL_STACK_SIZE * 2 )
+#define configTIMER_TASK_STACK_DEPTH    ( 512 )
 
 /* Set the following definitions to 1 to include the API function, or zero
 to exclude the API function. */
@@ -73,6 +73,7 @@ to exclude the API function. */
 #define INCLUDE_vTaskSuspend            1
 #define INCLUDE_vTaskDelayUntil         1
 #define INCLUDE_vTaskDelay              1
+#define INCLUDE_xTimerPendFunctionCall  1
 
 #if defined(THREADSAFE_MALLOC)
 #define INCLUDE_xTaskGetSchedulerState  1
diff --git a/radio/src/io/frsky_pxx2.h b/radio/src/io/frsky_pxx2.h
index 1ee9be85b..de13ff6ad 100644
--- a/radio/src/io/frsky_pxx2.h
+++ b/radio/src/io/frsky_pxx2.h
@@ -25,7 +25,7 @@
 #include "../fifo.h"
 #include "../pulses/pxx.h"
 
-class ModuleFifo : public Fifo<uint8_t, PXX2_FRAME_MAXLENGTH> {
+class ModuleFifo : public Fifo<uint8_t, INTMODULE_FIFO_SIZE> {
   public:
     bool getFrame(uint8_t * frame)
     {
@@ -74,7 +74,6 @@ class ModuleFifo : public Fifo<uint8_t, PXX2_FRAME_MAXLENGTH> {
       return ((crc >> 8) == crcLow) && ((crc & 0xFF) == crcHigh);
     }
 
-    uint32_t errors;
 };
 
 #endif
diff --git a/radio/src/pulses/pulses.cpp b/radio/src/pulses/pulses.cpp
index 2ef9b86ed..2bfec08f0 100755
--- a/radio/src/pulses/pulses.cpp
+++ b/radio/src/pulses/pulses.cpp
@@ -49,12 +49,63 @@
 #include "pulses/afhds2.h"
 #endif
 
+#include <FreeRTOS.h>
+#include <timers.h>
+
 uint8_t s_pulses_paused = 0;
 ModuleState moduleState[NUM_MODULES];
 InternalModulePulsesData intmodulePulsesData __DMA;
 ExternalModulePulsesData extmodulePulsesData __DMA;
 TrainerPulsesData trainerPulsesData __DMA;
 
+static TimerHandle_t telemetryTimer = nullptr;
+static StaticTimer_t telemetryTimerBuffer;
+
+void telemetryTimerCb(TimerHandle_t xTimer)
+{
+  (void)xTimer;
+  if (!s_pulses_paused) {
+    DEBUG_TIMER_START(debugTimerTelemetryWakeup);
+    telemetryWakeup();
+    DEBUG_TIMER_STOP(debugTimerTelemetryWakeup);
+  }
+}
+
+void startPulses()
+{
+  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. */
+      }
+    }
+  }
+
+  s_pulses_paused = false;
+
+#if defined(HARDWARE_INTERNAL_MODULE)
+  setupPulsesInternalModule();
+#endif
+
+#if defined(HARDWARE_EXTERNAL_MODULE)
+  setupPulsesExternalModule();
+#endif
+
+#if defined(HARDWARE_EXTRA_MODULE)
+  extramodulePpmStart();
+#endif
+}
+
+void stopPulses()
+{
+  s_pulses_paused = true;
+  for (uint8_t i = 0; i < NUM_MODULES; i++)
+    moduleState[i].protocol = PROTOCOL_CHANNELS_UNINITIALIZED;
+}
+
 void restartModule(uint8_t idx)
 {
   if (idx == INTERNAL_MODULE) {
diff --git a/radio/src/pulses/pulses.h b/radio/src/pulses/pulses.h
index 8988588ac..0ad3b2848 100644
--- a/radio/src/pulses/pulses.h
+++ b/radio/src/pulses/pulses.h
@@ -260,29 +260,8 @@ uint8_t actualAfhdsRunPower(int moduleIndex);
 #endif
 void extramodulePpmStart();
 
-inline void startPulses()
-{
-  s_pulses_paused = false;
-
-#if defined(HARDWARE_INTERNAL_MODULE)
-  setupPulsesInternalModule();
-#endif
-
-#if defined(HARDWARE_EXTERNAL_MODULE)
-  setupPulsesExternalModule();
-#endif
-
-#if defined(HARDWARE_EXTRA_MODULE)
-  extramodulePpmStart();
-#endif
-}
-
-inline void stopPulses()
-{
-  s_pulses_paused = true;
-  for (uint8_t i = 0; i < NUM_MODULES; i++)
-    moduleState[i].protocol = PROTOCOL_CHANNELS_UNINITIALIZED;
-}
+void startPulses();
+void stopPulses();
 
 inline bool pulsesStarted()
 {
diff --git a/radio/src/tasks.cpp b/radio/src/tasks.cpp
index 5ec392519..495a11718 100644
--- a/radio/src/tasks.cpp
+++ b/radio/src/tasks.cpp
@@ -100,12 +100,6 @@ void execMixerFrequentActions()
 #if defined(BLUETOOTH)
   bluetooth.wakeup();
 #endif
-
-  if (!s_pulses_paused) {
-    DEBUG_TIMER_START(debugTimerTelemetryWakeup);
-    telemetryWakeup();
-    DEBUG_TIMER_STOP(debugTimerTelemetryWakeup);
-  }
 }
 
 TASK_FUNCTION(mixerTask)