diff --git a/components/can/CAN.c b/components/can/CAN.c
index ffd55bf..31e30fa 100644
--- a/components/can/CAN.c
+++ b/components/can/CAN.c
@@ -42,12 +42,13 @@
 #include "CAN_config.h"
 
 
-static void CAN_read_frame();
+static void CAN_read_frame(BaseType_t* pfYieldRequired);
 static void CAN_isr(void *arg_p);
 
 
 static void CAN_isr(void *arg_p){
 
+BaseType_t fYieldRequired = pdFALSE;
 	//Interrupt flag buffer
 	__CAN_IRQ_t interrupt;
 
@@ -61,7 +62,7 @@ static void CAN_isr(void *arg_p){
 
     // Handle RX frame available interrupt
     if ((interrupt & __CAN_IRQ_RX) != 0)
-    	CAN_read_frame();
+    	CAN_read_frame(&fYieldRequired);
 
     // Handle error interrupts.
     if ((interrupt & (__CAN_IRQ_ERR						//0x4
@@ -73,9 +74,11 @@ static void CAN_isr(void *arg_p){
 	)) != 0) {
     	/*handler*/
     }
+	if (fYieldRequired == pdTRUE)
+		  portYIELD_FROM_ISR();
 }
 
-static void CAN_read_frame(){
+static void CAN_read_frame(BaseType_t* pfYieldRequired){
 
 	//byte iterator
 	uint8_t __byte_i;
@@ -87,6 +90,7 @@ static void CAN_read_frame(){
     if (CAN_cfg.rx_queue == NULL){
         // Let the hardware know the frame has been read.
         MODULE_CAN->CMR.B.RRB=1;
+	*pfYieldRequired = pdFALSE;
         return;
     }
 
@@ -118,7 +122,7 @@ static void CAN_read_frame(){
     }
 
     //send frame to input queue
-    xQueueSendFromISR(CAN_cfg.rx_queue,&__frame,0);
+    xQueueSendFromISR(CAN_cfg.rx_queue,&__frame, pfYieldRequired);
 
     //Let the hardware know the frame has been read.
     MODULE_CAN->CMR.B.RRB=1;