diff --git a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp
index c1514d8782..9902e3e1f1 100644
--- a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp
+++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp
@@ -20,9 +20,9 @@
 #elif UAVCAN_STM32_FREERTOS
 # include <stm32f4xx.h>
 # include <cmsis_os.h>
-  #ifndef MAX_SEMAPHORE_COUNT
-    #define MAX_SEMAPHORE_COUNT      50
-  #endif
+#   ifndef MAX_SEMAPHORE_COUNT
+#   define MAX_SEMAPHORE_COUNT      50
+#endif
 #else
 # error "Unknown OS"
 #endif
@@ -186,12 +186,14 @@ public:
     void signal();
 
     void signalFromInterrupt();
+
+    void yieldFromISR();
 };
 
 class Mutex
 {
     SemaphoreHandle_t mtx_;
-    BaseType_t xHigherPriorityTaskWoken;
+
 public:
     Mutex(void)
     {
diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp
index 24cdcb0268..9f246427af 100644
--- a/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp
+++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp
@@ -116,6 +116,11 @@ void BusEvent::signalFromInterrupt()
     portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
 }
 
+void BusEvent::yieldFromISR()
+{
+    portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
+}
+
 /*
  * Mutex
  */