Priority Scheduler FIX: Preemption does not occure while idle thread is active

miosix/kernel/scheduler/priority/priority_scheduler.cpp

@@ -201,14 +201,18 @@ void PriorityScheduler::IRQsetIdleThread(Thread *idleThread)
     idle=idleThread;
 }
 
-static void setNextPreemption(){
+static void setNextPreemption(bool curIsIdleThread){
     static long long preemptionPeriodTicks = tc->ns2tick(preemptionPeriodNs);
+    if (curIsIdleThread){
+        timer.IRQsetNextInterrupt(firstSleepItemTicks);
+    }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;
 }