diff --git a/radio/src/FreeRTOSConfig.h b/radio/src/FreeRTOSConfig.h
index a2a81e6cf..2213a2187 100644
--- a/radio/src/FreeRTOSConfig.h
+++ b/radio/src/FreeRTOSConfig.h
@@ -35,6 +35,8 @@
 #define configUSE_TRACE_FACILITY        0 // 1 rco: save some memory? 
 #define configUSE_16_BIT_TICKS          0
 #define configIDLE_SHOULD_YIELD         1
+// Use notifications
+#define configTASK_NOTIFICATION_ARRAY_ENTRIES    2
 #define configUSE_MUTEXES               0
 #define configQUEUE_REGISTRY_SIZE       0
 #define configCHECK_FOR_STACK_OVERFLOW  0
diff --git a/radio/src/mixer_scheduler.cpp b/radio/src/mixer_scheduler.cpp
index 2a0824cf5..ee0a7198a 100644
--- a/radio/src/mixer_scheduler.cpp
+++ b/radio/src/mixer_scheduler.cpp
@@ -24,7 +24,6 @@
 #if !defined(SIMU)
 
 // Global trigger flag
-RTOS_FLAG_HANDLE mixerFlag;
 
 // Mixer schedule
 struct MixerSchedule {
@@ -52,7 +51,6 @@ uint16_t getMixerSchedulerPeriod()
 
 void mixerSchedulerInit()
 {
-  RTOS_CREATE_MUTEX(mixerFlag);
   memset(mixerSchedules, 0, sizeof(mixerSchedules));
 }
 
@@ -68,15 +66,48 @@ void mixerSchedulerSetPeriod(uint8_t moduleIdx, uint16_t periodUs)
   mixerSchedules[moduleIdx].period = periodUs;
 }
 
-
 bool mixerSchedulerWaitForTrigger(uint8_t timeoutMs)
 {
-  return RTOS_WAIT_FLAG(mixerFlag, timeoutMs);
+  uint32_t ulNotificationValue;
+  const TickType_t xMaxBlockTime = pdMS_TO_TICKS( timeoutMs );
+
+  /* Wait to be notified that the transmission is complete.  Note
+     the first parameter is pdTRUE, which has the effect of clearing
+     the task's notification value back to 0, making the notification
+     value act like a binary (rather than a counting) semaphore.  */
+  ulNotificationValue = ulTaskNotifyTakeIndexed( 1 /* task notification index */,
+                                                 pdTRUE,
+                                                 xMaxBlockTime );
+
+  if( ulNotificationValue == 1 ) {
+    /* The transmission ended as expected. */
+    return false;
+
+  } else {
+    /* The call to ulTaskNotifyTake() timed out. */
+    return true;
+  }
 }
 
 void mixerSchedulerISRTrigger()
 {
-  RTOS_ISR_SET_FLAG(mixerFlag);
+  BaseType_t xHigherPriorityTaskWoken = pdFALSE;
+
+  /* At this point xTaskToNotify should not be NULL as
+     a transmission was in progress. */
+  configASSERT( mixerTaskId.rtos_handle != NULL );
+
+  /* Notify the task that the transmission is complete. */
+  vTaskNotifyGiveIndexedFromISR( mixerTaskId.rtos_handle,
+                                 1 /* task notification index */,
+                                 &xHigherPriorityTaskWoken );
+
+  /* If xHigherPriorityTaskWoken is now set to pdTRUE then a
+     context switch should be performed to ensure the interrupt
+     returns directly to the highest priority task.  The macro used
+     for this purpose is dependent on the port in use and may be
+     called portEND_SWITCHING_ISR(). */
+  portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
 }
 
 #endif
diff --git a/radio/src/rtos.h b/radio/src/rtos.h
index 36e35ff6a..2e65801d7 100644
--- a/radio/src/rtos.h
+++ b/radio/src/rtos.h
@@ -75,26 +75,6 @@ extern "C++" {
       pthread_mutex_unlock(&mutex);
   }
 
-  static inline void RTOS_CREATE_FLAG(RTOS_FLAG_HANDLE flag)
-  {
-  }
-
-  static inline void RTOS_SET_FLAG(RTOS_FLAG_HANDLE flag)
-  {
-  }
-
-  static inline void RTOS_CLEAR_FLAG(RTOS_FLAG_HANDLE flag)
-  {
-  }
-
-  static inline bool RTOS_WAIT_FLAG(RTOS_FLAG_HANDLE flag, uint32_t timeout)
-  {
-    simuSleep(timeout);
-    return false;
-  }
-
-  #define RTOS_ISR_SET_FLAG RTOS_SET_FLAG
-
   template<int SIZE>
   class FakeTaskStack
   {
@@ -169,7 +149,6 @@ inline void RTOS_CREATE_TASK(pthread_t &taskId, void * (*task)(void *), const ch
 
   #define RTOS_MS_PER_TICK portTICK_PERIOD_MS
 
-  // TODO:
   typedef struct {
     TaskHandle_t rtos_handle;
     StaticTask_t task_struct;
@@ -217,25 +196,29 @@ inline void RTOS_CREATE_TASK(pthread_t &taskId, void * (*task)(void *), const ch
   #define RTOS_CREATE_TASK(h,task,name,stackStruct,stackSize,prio) \
     _RTOS_CREATE_TASK(&h,task,name,stackStruct.stack,stackSize,prio)
   
-
-#ifdef __cplusplus
-  static inline void RTOS_CREATE_MUTEX(RTOS_MUTEX_HANDLE& h)
+  static inline void _RTOS_CREATE_MUTEX(RTOS_MUTEX_HANDLE* h)
   {
-    h.rtos_handle = xSemaphoreCreateBinaryStatic(&h.mutex_struct);
-    xSemaphoreGive(h.rtos_handle);
+    h->rtos_handle = xSemaphoreCreateBinaryStatic(&h->mutex_struct);
+    xSemaphoreGive(h->rtos_handle);
   }
 
-  static inline void RTOS_LOCK_MUTEX(RTOS_MUTEX_HANDLE& h)
+  #define RTOS_CREATE_MUTEX(handle) _RTOS_CREATE_MUTEX(&handle)
+
+  static inline void _RTOS_LOCK_MUTEX(RTOS_MUTEX_HANDLE* h)
   {
-    xSemaphoreTake(h.rtos_handle, portMAX_DELAY);
+    xSemaphoreTake(h->rtos_handle, portMAX_DELAY);
   }
 
-  static inline void RTOS_UNLOCK_MUTEX(RTOS_MUTEX_HANDLE& h)
-  {
-    xSemaphoreGive(h.rtos_handle);
-  }
-#endif  // __cplusplus
+  #define RTOS_LOCK_MUTEX(handle) _RTOS_LOCK_MUTEX(&handle)
 
+  static inline void _RTOS_UNLOCK_MUTEX(RTOS_MUTEX_HANDLE* h)
+  {
+    xSemaphoreGive(h->rtos_handle);
+  }
+
+  #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;
@@ -262,25 +245,29 @@ inline void RTOS_CREATE_TASK(pthread_t &taskId, void * (*task)(void *), const ch
   //#define RTOS_SET_FLAG(flag)           (void)CoSetFlag(flag)
   //#define RTOS_CLEAR_FLAG(flag)         (void)CoClearFlag(flag)
 
-#ifdef __cplusplus
   // returns true if timeout
-  static inline bool RTOS_WAIT_FLAG(RTOS_FLAG_HANDLE& flag, uint32_t timeout)
+  static inline bool _RTOS_WAIT_FLAG(RTOS_FLAG_HANDLE* flag, uint32_t timeout)
   {
-    return xSemaphoreTake(flag.rtos_handle, timeout * RTOS_MS_PER_TICK)
+    return xSemaphoreTake(flag->rtos_handle, timeout * RTOS_MS_PER_TICK)
       == pdFALSE;
   }
 
-  static inline void RTOS_ISR_SET_FLAG(RTOS_FLAG_HANDLE& flag)
+  #define RTOS_WAIT_FLAG(flag,timeout) _RTOS_WAIT_FLAG(&flag,timeout)
+
+  static inline void _RTOS_ISR_SET_FLAG(RTOS_FLAG_HANDLE* flag)
   {
     BaseType_t xHigherPriorityTaskWoken = pdFALSE;
 
     // Give semaphore back from ISR to trigger a task waiting for it
-    xSemaphoreGiveFromISR( flag.rtos_handle, &xHigherPriorityTaskWoken );
+    xSemaphoreGiveFromISR( flag->rtos_handle, &xHigherPriorityTaskWoken );
 
     // If xHigherPriorityTaskWoken was set to true we should yield
     portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
   }
 
+  #define RTOS_ISR_SET_FLAG(flag) _RTOS_ISR_SET_FLAG(&flag)
+
+#ifdef __cplusplus
   template<int SIZE>
   class TaskStack
   {
diff --git a/radio/src/targets/common/arm/stm32/heartbeat_driver.cpp b/radio/src/targets/common/arm/stm32/heartbeat_driver.cpp
index cdb373de0..108f3e7b7 100644
--- a/radio/src/targets/common/arm/stm32/heartbeat_driver.cpp
+++ b/radio/src/targets/common/arm/stm32/heartbeat_driver.cpp
@@ -51,7 +51,6 @@ void init_intmodule_heartbeat()
 
   NVIC_SetPriority(INTMODULE_HEARTBEAT_EXTI_IRQn,
                    // Highest priority interrupt
-                   // TODO: replace with a define...
                    configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY);
 
   NVIC_EnableIRQ(INTMODULE_HEARTBEAT_EXTI_IRQn);
diff --git a/radio/src/targets/common/arm/stm32/mixer_scheduler_driver.cpp b/radio/src/targets/common/arm/stm32/mixer_scheduler_driver.cpp
index 69858da04..9d399d447 100644
--- a/radio/src/targets/common/arm/stm32/mixer_scheduler_driver.cpp
+++ b/radio/src/targets/common/arm/stm32/mixer_scheduler_driver.cpp
@@ -39,7 +39,6 @@ void mixerSchedulerStart()
 
   NVIC_EnableIRQ(MIXER_SCHEDULER_TIMER_IRQn);
   NVIC_SetPriority(MIXER_SCHEDULER_TIMER_IRQn,
-                   // TODO: replace with a define...
                    configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY);
 
   MIXER_SCHEDULER_TIMER->SR   &= TIM_SR_UIF;   // clear interrupt flag