Merge pull request #129 from pdima/skip_late_updates

Skip task/timer updated if late for more than a few periods
orocos-toolchain · Aug 10, 2016 · 1fc4a17 · 1fc4a17
1 parent cd3c19e
commit 1fc4a17
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 2 deletions.
diff --git 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
@@ -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;