diff --git a/rtt/os/Timer.cpp b/rtt/os/Timer.cpp index 31e611420..c9a100569 100644 --- a/rtt/os/Timer.cpp +++ b/rtt/os/Timer.cpp @@ -82,7 +82,8 @@ namespace RTT { // Wait int ret = 0; - if ( wake_up_time > rtos_get_time_ns() ) + Time now = rtos_get_time_ns(); + if ( wake_up_time > now ) ret = msem.waitUntil( wake_up_time ); // case of no timers or running timers else ret = -1; // case of timer overrun. @@ -99,6 +100,11 @@ namespace RTT { TimerIds::iterator tim = mtimers.begin() + next_timer_id; if ( tim->period ) { // periodic timer + // if late by more than 4 periods, skip late updates + int maxDelayInPeriods = 4; + if (now - tim->expires > tim->period*maxDelayInPeriods) { + tim->expires += tim->period*((now - tim->expires) / tim->period); + } tim->expires += tim->period; } else { // aperiodic timer diff --git a/rtt/os/gnulinux/fosi_internal.cpp b/rtt/os/gnulinux/fosi_internal.cpp index c1355a719..b5a72bb06 100644 --- a/rtt/os/gnulinux/fosi_internal.cpp +++ b/rtt/os/gnulinux/fosi_internal.cpp @@ -254,9 +254,16 @@ namespace RTT if (task->wait_policy == ORO_WAIT_ABS) { + // in the case of overrun by more than 4 periods, + // skip all the updates before now, with the next update aligned to period + int maxDelayInPeriods = 4; + NANO_TIME period = task->period; + if (now - wake > maxDelayInPeriods*period) { + period = period * ((now - wake) / period); + } // program next period: // 1. convert period to timespec - TIME_SPEC ts = ticks2timespec( nano2ticks( task->period) ); + TIME_SPEC ts = ticks2timespec( nano2ticks(period) ); // 2. Add ts to periodMark (danger: tn guards for overflows!) NANO_TIME tn = (task->periodMark.tv_nsec + ts.tv_nsec); task->periodMark.tv_nsec = tn % 1000000000LL;