diff --git a/radio/src/FreeRTOSConfig.h b/radio/src/FreeRTOSConfig.h
index e8a0706f055..a751c99bccc 100644
--- a/radio/src/FreeRTOSConfig.h
+++ b/radio/src/FreeRTOSConfig.h
@@ -75,11 +75,12 @@ extern uint32_t SystemCoreClock;
 
 /* Set the following definitions to 1 to include the API function, or zero
 to exclude the API function. */
-#define INCLUDE_vTaskDelete             1
-#define INCLUDE_vTaskSuspend            1
-#define INCLUDE_vTaskDelayUntil         1
-#define INCLUDE_vTaskDelay              1
-#define INCLUDE_xTimerPendFunctionCall  1
+#define INCLUDE_vTaskDelete                 1
+#define INCLUDE_vTaskSuspend                1
+#define INCLUDE_vTaskDelayUntil             1
+#define INCLUDE_vTaskDelay                  1
+#define INCLUDE_xTimerPendFunctionCall      1
+#define INCLUDE_uxTaskGetStackHighWaterMark 1
 
 #if defined(THREADSAFE_MALLOC)
 #define INCLUDE_xTaskGetSchedulerState  1
diff --git a/radio/src/cli.cpp b/radio/src/cli.cpp
index c2bc5ce0a18..4d65d472038 100644
--- a/radio/src/cli.cpp
+++ b/radio/src/cli.cpp
@@ -47,7 +47,7 @@
 #define CLI_PRINT_BUFFER_SIZE 128
 
 RTOS_TASK_HANDLE cliTaskId;
-RTOS_DEFINE_STACK(cliStack, CLI_STACK_SIZE);
+RTOS_DEFINE_STACK(cliTaskId, cliStack, CLI_STACK_SIZE);
 
 static uint8_t cliRxBufferStorage[CLI_RX_BUFFER_SIZE];
 static StaticStreamBuffer_t cliRxBufferStatic;
@@ -843,7 +843,7 @@ int cliTrace(const char ** argv)
 
 int cliStackInfo(const char ** argv)
 {
-  cliSerialPrint("[MAIN] %d available / %d bytes", stackAvailable()*4, stackSize()*4);
+  cliSerialPrint("[MAIN] %d available / %d bytes", mainStackAvailable()*4, stackSize()*4);
   cliSerialPrint("[MENUS] %d available / %d bytes", menusStack.available()*4, menusStack.size());
   cliSerialPrint("[MIXER] %d available / %d bytes", mixerStack.available()*4, mixerStack.size());
   cliSerialPrint("[AUDIO] %d available / %d bytes", audioStack.available()*4, audioStack.size());
diff --git a/radio/src/gui/212x64/view_statistics.cpp b/radio/src/gui/212x64/view_statistics.cpp
index 270c808078a..759f9dc4fcf 100644
--- a/radio/src/gui/212x64/view_statistics.cpp
+++ b/radio/src/gui/212x64/view_statistics.cpp
@@ -192,7 +192,7 @@ void menuStatisticsDebug(event_t event)
   lcdDrawText(lcdLastRightPos+2, y+1, "[A]", SMLSIZE);
   lcdDrawNumber(lcdLastRightPos, y, audioStack.available(), LEFT);
   lcdDrawText(lcdLastRightPos+2, y+1, "[I]", SMLSIZE);
-  lcdDrawNumber(lcdLastRightPos, y, stackAvailable(), LEFT);
+  lcdDrawNumber(lcdLastRightPos, y, mainStackAvailable(), LEFT);
   y += FH;
 
 #if defined(DEBUG_LATENCY)
diff --git a/radio/src/opentx.cpp b/radio/src/opentx.cpp
index d5965b1c745..5823563070a 100644
--- a/radio/src/opentx.cpp
+++ b/radio/src/opentx.cpp
@@ -1867,10 +1867,6 @@ int main()
 
   boardInit();
 
-#if !defined(SIMU)
-  stackPaint();
-#endif
-
 #if defined(PCBHORUS)
   if (!IS_FIRMWARE_COMPATIBLE_WITH_BOARD()) {
     runFatalErrorScreen(STR_WRONG_PCBREV);
diff --git a/radio/src/rtos.h b/radio/src/rtos.h
index 827942e45f1..e29b9b0c68b 100644
--- a/radio/src/rtos.h
+++ b/radio/src/rtos.h
@@ -76,10 +76,10 @@ extern "C++" {
   }
 
   template<int SIZE>
-  class FakeTaskStack
+  class TaskStack
   {
     public:
-      FakeTaskStack()
+      TaskStack()
       {
       }
 
@@ -97,7 +97,7 @@ extern "C++" {
         return SIZE / 2;
       }
   };
-  #define RTOS_DEFINE_STACK(name, size) FakeTaskStack<size> name
+  #define RTOS_DEFINE_STACK(taskHandle, name, size) TaskStack<size> name
 
   #define TASK_FUNCTION(task)           void* task(void *)
 
@@ -110,7 +110,7 @@ extern "C++" {
   }
 
 template<int SIZE>
-inline void RTOS_CREATE_TASK(pthread_t &taskId, void * (*task)(void *), const char * name, FakeTaskStack<SIZE> &, unsigned size = 0, unsigned priority = 0)
+inline void RTOS_CREATE_TASK(pthread_t &taskId, void * (*task)(void *), const char * name, TaskStack<SIZE> &, unsigned size = 0, unsigned priority = 0)
   {
     UNUSED(size);
     UNUSED(priority);
@@ -119,7 +119,7 @@ inline void RTOS_CREATE_TASK(pthread_t &taskId, void * (*task)(void *), const ch
 
   #define TASK_RETURN()                 return nullptr
 
-  constexpr uint32_t stackAvailable()
+  constexpr uint32_t mainStackAvailable()
   {
     return 500;
   }
@@ -218,7 +218,6 @@ inline void RTOS_CREATE_TASK(pthread_t &taskId, void * (*task)(void *), const ch
 
   #define RTOS_UNLOCK_MUTEX(handle) _RTOS_UNLOCK_MUTEX(&handle)
 
-  //TODO: replace with FreeRTOS functions
   static inline uint32_t getStackAvailable(void * address, uint32_t size)
   {
     uint32_t * array = (uint32_t *)address;
@@ -236,7 +235,7 @@ inline void RTOS_CREATE_TASK(pthread_t &taskId, void * (*task)(void *), const ch
     return ((unsigned char *)&_estack - (unsigned char *)&_main_stack_start) / 4;
   }
 
-  static inline uint32_t stackAvailable()
+  static inline uint32_t mainStackAvailable()
   {
     return getStackAvailable(&_main_stack_start, stackSize());
   }
@@ -272,15 +271,8 @@ inline void RTOS_CREATE_TASK(pthread_t &taskId, void * (*task)(void *), const ch
   class TaskStack
   {
     public:
-      TaskStack()
-      {
-      }
-
-      void paint()
-      {
-        for (uint32_t i=0; i<SIZE; i++) {
-          stack[i] = 0x55555555;
-        }
+      TaskStack(RTOS_TASK_HANDLE *h) {
+        this->h = h;
       }
 
       uint32_t size()
@@ -290,10 +282,12 @@ inline void RTOS_CREATE_TASK(pthread_t &taskId, void * (*task)(void *), const ch
 
       uint32_t available()
       {
-        return getStackAvailable(stack, SIZE);
+        return uxTaskGetStackHighWaterMark(h->rtos_handle);
       }
 
       StackType_t stack[SIZE];
+    protected:
+      RTOS_TASK_HANDLE *h;
   };
 #endif // __cplusplus
 
@@ -308,7 +302,7 @@ inline void RTOS_CREATE_TASK(pthread_t &taskId, void * (*task)(void *), const ch
   }
 
   // stack must be aligned to 8 bytes otherwise printf for %f does not work!
-  #define RTOS_DEFINE_STACK(name, size) TaskStack<size> __ALIGNED(8) name __CCMRAM
+  #define RTOS_DEFINE_STACK(taskHandle, name, size) TaskStack<size> __ALIGNED(8) name __CCMRAM (&taskHandle) 
 
   #define TASK_FUNCTION(task)           void task(void *)
   #define TASK_RETURN()                 vTaskDelete(nullptr)
diff --git a/radio/src/tasks.cpp b/radio/src/tasks.cpp
index 7fdec9a74f8..bed5b90187f 100644
--- a/radio/src/tasks.cpp
+++ b/radio/src/tasks.cpp
@@ -24,27 +24,17 @@
 #include "timers_driver.h"
 
 RTOS_TASK_HANDLE menusTaskId;
-RTOS_DEFINE_STACK(menusStack, MENUS_STACK_SIZE);
+RTOS_DEFINE_STACK(menusTaskId, menusStack, MENUS_STACK_SIZE);
 
 RTOS_TASK_HANDLE mixerTaskId;
-RTOS_DEFINE_STACK(mixerStack, MIXER_STACK_SIZE);
+RTOS_DEFINE_STACK(mixerTaskId, mixerStack, MIXER_STACK_SIZE);
 
 RTOS_TASK_HANDLE audioTaskId;
-RTOS_DEFINE_STACK(audioStack, AUDIO_STACK_SIZE);
+RTOS_DEFINE_STACK(audioTaskId, audioStack, AUDIO_STACK_SIZE);
 
 RTOS_MUTEX_HANDLE audioMutex;
 RTOS_MUTEX_HANDLE mixerMutex;
 
-void stackPaint()
-{
-  menusStack.paint();
-  mixerStack.paint();
-  audioStack.paint();
-#if defined(CLI) && !defined(SIMU)
-  cliStack.paint();
-#endif
-}
-
 volatile uint16_t timeForcePowerOffPressed = 0;
 
 bool isForcePowerOffRequested()
diff --git a/radio/src/tasks.h b/radio/src/tasks.h
index 03c9ee24eff..17e8d13f795 100644
--- a/radio/src/tasks.h
+++ b/radio/src/tasks.h
@@ -46,22 +46,23 @@
 #define CLI_TASK_PRIO          (1)
 #endif
 
-extern RTOS_TASK_HANDLE menusTaskId;
-extern RTOS_DEFINE_STACK(menusStack, MENUS_STACK_SIZE);
 
 extern RTOS_MUTEX_HANDLE mixerMutex;
+
+extern RTOS_TASK_HANDLE menusTaskId;
+extern TaskStack<MENUS_STACK_SIZE> menusStack;
+
 extern RTOS_TASK_HANDLE mixerTaskId;
-extern RTOS_DEFINE_STACK(mixerStack, MIXER_STACK_SIZE);
+extern TaskStack<MIXER_STACK_SIZE> mixerStack;
 
 extern RTOS_TASK_HANDLE audioTaskId;
-extern RTOS_DEFINE_STACK(audioStack, AUDIO_STACK_SIZE);
+extern TaskStack<AUDIO_STACK_SIZE> audioStack;
 
 #if defined(CLI)
 extern RTOS_TASK_HANDLE cliTaskId;
-extern RTOS_DEFINE_STACK(cliStack, CLI_STACK_SIZE);
+extern TaskStack<CLI_STACK_SIZE> cliStack;
 #endif
 
-void stackPaint();
 void tasksStart();
 
 extern volatile uint16_t timeForcePowerOffPressed;