diff --git a/miosix/kernel/scheduler/priority/priority_scheduler.cpp b/miosix/kernel/scheduler/priority/priority_scheduler.cpp
index 1c2395120aaa93effa8b85dd35a8c4d31f7ae2ed..c146898c537dc50bae36afb610891c11b84cfd19 100644
--- a/miosix/kernel/scheduler/priority/priority_scheduler.cpp
+++ b/miosix/kernel/scheduler/priority/priority_scheduler.cpp
@@ -201,13 +201,17 @@ void PriorityScheduler::IRQsetIdleThread(Thread *idleThread)
     idle=idleThread;
 }
 
-static void setNextPreemption(){
+static void setNextPreemption(bool curIsIdleThread){
     static long long preemptionPeriodTicks = tc->ns2tick(preemptionPeriodNs);
-    long long nextPeriodicPreemption = timer.IRQgetCurrentTick() + preemptionPeriodTicks;   
-    if (firstSleepItemTicks < nextPeriodicPreemption )
+    if (curIsIdleThread){
         timer.IRQsetNextInterrupt(firstSleepItemTicks);
-    else
-        timer.IRQsetNextInterrupt(nextPeriodicPreemption);
+    }else{
+        long long nextPeriodicPreemption = timer.IRQgetCurrentTick() + preemptionPeriodTicks;   
+        if (firstSleepItemTicks < nextPeriodicPreemption )
+            timer.IRQsetNextInterrupt(firstSleepItemTicks);
+        else
+            timer.IRQsetNextInterrupt(nextPeriodicPreemption);
+    }
 }
 
 unsigned int PriorityScheduler::IRQfindNextThread()
@@ -239,7 +243,7 @@ unsigned int PriorityScheduler::IRQfindNextThread()
                 //Rotate to next thread so that next time the list is walked
                 //a different thread, if available, will be chosen first
                 thread_list[i]=temp;
-                setNextPreemption();
+                setNextPreemption(false);
                 return preemptionPeriodNs;
             } else temp=temp->schedData.next;
             if(temp==thread_list[i]->schedData.next) break;
@@ -251,7 +255,7 @@ unsigned int PriorityScheduler::IRQfindNextThread()
     #ifdef WITH_PROCESSES
     MPUConfiguration::IRQdisable();
     #endif //WITH_PROCESSES
-    setNextPreemption();
+    setNextPreemption(true);
     return preemptionPeriodNs;
 }