diff --git a/freertos-rust/src/freertos/shim.c b/freertos-rust/src/freertos/shim.c
index b5e544f..99a9240 100644
--- a/freertos-rust/src/freertos/shim.c
+++ b/freertos-rust/src/freertos/shim.c
@@ -277,8 +277,8 @@ UBaseType_t freertos_rs_queue_messages_waiting(QueueHandle_t queue) {
 	return uxQueueMessagesWaiting( queue );
 }
 
-void freertos_rs_isr_yield() {
-	portYIELD();
+void freertos_rs_isr_yield(BaseType_t xHigherPriorityTaskWoken) {
+	portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
 }
 
 TickType_t freertos_rs_max_wait() {
diff --git a/freertos-rust/src/isr.rs b/freertos-rust/src/isr.rs
index 4a35268..7dd21e9 100644
--- a/freertos-rust/src/isr.rs
+++ b/freertos-rust/src/isr.rs
@@ -27,10 +27,8 @@ impl InterruptContext {
 
 impl Drop for InterruptContext {
     fn drop(&mut self) {
-        if self.x_higher_priority_task_woken == 1 {
-            unsafe {
-                freertos_rs_isr_yield();
-            }
+        unsafe {
+            freertos_rs_isr_yield(self.x_higher_priority_task_woken);
         }
     }
 }
diff --git a/freertos-rust/src/shim.rs b/freertos-rust/src/shim.rs
index 150fdff..d4cdecc 100644
--- a/freertos-rust/src/shim.rs
+++ b/freertos-rust/src/shim.rs
@@ -81,7 +81,7 @@ extern "C" {
         item: FreeRtosVoidPtr,
         xHigherPriorityTaskWoken: FreeRtosBaseTypeMutPtr,
     ) -> FreeRtosUBaseType;
-    pub fn freertos_rs_isr_yield();
+    pub fn freertos_rs_isr_yield(xHigherPriorityTaskWoken: FreeRtosBaseType);
 
     pub fn freertos_rs_task_notify_take(clear_count: u8, wait: FreeRtosTickType) -> u32;
     pub fn freertos_rs_task_notify_wait(