activity: implement abs/rel wait and overrun detection.

Signed-off-by: Peter Soetens <[email protected]>

rtt/Activity.cpp

@@ -55,19 +55,19 @@ namespace RTT
 
     Activity::Activity(RunnableInterface* _r, const std::string& name )
         : ActivityInterface(_r), os::Thread(ORO_SCHED_OTHER, RTT::os::LowestPriority, 0.0, 0, name ),
-          update_period(0.0), mtimeout(false)
+          update_period(0.0), mtimeout(false), mstopRequested(false), mabswaitpolicy(false)
     {
     }
 
     Activity::Activity(int priority, RunnableInterface* r, const std::string& name )
         : ActivityInterface(r), os::Thread(ORO_SCHED_RT, priority, 0.0, 0, name ),
-          update_period(0.0), mtimeout(false)
+          update_period(0.0), mtimeout(false), mstopRequested(false), mabswaitpolicy(false)
     {
     }
 
     Activity::Activity(int priority, Seconds period, RunnableInterface* r, const std::string& name )
         : ActivityInterface(r), os::Thread(ORO_SCHED_RT, priority, period, 0, name ),
-          update_period(period), mtimeout(false)
+          update_period(period), mtimeout(false), mstopRequested(false), mabswaitpolicy(false)
     {
         // We pass the requested period to the constructor to not confuse users with log messages.
         // Then we clear it immediately again in order to force the Thread implementation to
@@ -82,7 +82,7 @@ namespace RTT
 
      Activity::Activity(int scheduler, int priority, Seconds period, RunnableInterface* r, const std::string& name )
          : ActivityInterface(r), os::Thread(scheduler, priority, period, 0, name ),
-           update_period(period), mtimeout(false)
+           update_period(period), mtimeout(false), mstopRequested(false), mabswaitpolicy(false)
      {
          // We pass the requested period to the constructor to not confuse users with log messages.
          // Then we clear it immediately again in order to force the Thread implementation to
@@ -92,7 +92,7 @@ namespace RTT
 
      Activity::Activity(int scheduler, int priority, Seconds period, unsigned cpu_affinity, RunnableInterface* r, const std::string& name )
      : ActivityInterface(r), os::Thread(scheduler, priority, period, cpu_affinity, name ),
-       update_period(period), mtimeout(false)
+       update_period(period), mtimeout(false), mstopRequested(false), mabswaitpolicy(false)
      {
          // We pass the requested period to the constructor to not confuse users with log messages.
          // Then we clear it immediately again in order to force the Thread implementation to
@@ -165,7 +165,7 @@ namespace RTT
 
     void Activity::loop() {
         nsecs wakeup = 0;
-
+        int overruns = 0;
         while ( true ) {
             // since update_period may be changed at any time, we need to recheck it each time:
             if ( update_period > 0.0) {
@@ -209,9 +209,22 @@ namespace RTT
                 bool time_elapsed = ! msg_cond.wait_until(msg_lock,wakeup);
 
                 if (time_elapsed) {
+                    nsecs nsperiod = Seconds_to_nsecs(update_period);
+                    wakeup = wakeup + nsperiod;
                     // calculate next wakeup point, overruns causes skips:
-                    while ( wakeup < os::TimeService::Instance()->getNSecs() )
-                        wakeup = wakeup + Seconds_to_nsecs(update_period);
+                    nsecs now = os::TimeService::Instance()->getNSecs();
+                    if ( wakeup < now )
+                    {
+                        ++overruns;
+                        if (overruns == maxOverRun)
+                            break; // break while(true)
+                    }
+                    else if (overruns != 0) {
+                        --overruns;
+                    }
+                    if ( !mabswaitpolicy && wakeup < now ) {
+                        wakeup = wakeup + ((now-wakeup)/nsperiod+1)*nsperiod; // assumes that (now-wakeup)/nsperiod rounds down !
+                    }
                     mtimeout = true;
                 }
             }
@@ -220,6 +233,16 @@ namespace RTT
                 return;
             }
         }
+        if (overruns == maxOverRun)
+        {
+            this->emergencyStop();
+            log(Critical) << rtos_task_get_name(this->getTask())
+                    << " got too many periodic overruns in step() ("
+                    << overruns << " times), stopped Thread !"
+                    << endlog();
+            log(Critical) << " See Thread::setMaxOverrun() for info."
+                    << endlog();
+        }
     }
 
     bool Activity::breakLoop() {
@@ -309,4 +332,12 @@ namespace RTT
         return Thread::setCpuAffinity(cpu);
     }
 
+    void Activity::setWaitPeriodPolicy(int p)
+    {
+        if ( p == ORO_WAIT_ABS)
+            mabswaitpolicy = true;
+        else
+            mabswaitpolicy = false;
+    }
+
 }

rtt/Activity.hpp

@@ -50,7 +50,7 @@ namespace RTT
 {
 
     /**
-     * @brief An Activity is an object that represents a thread.
+     * @brief An Activity executes a RunnableInterface object in a (periodic) thread.
      *
      * This object implements the base::ActivityInterface and maps that to an
      * OS thread, using the RTT::os::Thread class. One Activity object
@@ -60,6 +60,11 @@ namespace RTT
      * When provided one, it will execute a base::RunnableInterface object, or the equivalent methods in
      * it's own interface when none is given.
      *
+     * For a periodic Activity, when it misses its deadline because user code
+     * take too long to execute, it will skip the required number of periodic
+     * execution steps in order to be back on time. This is the ORO_WAIT_REL wait
+     * policy and can be changed by calling setWaitPeriodPolicy(ORO_WAIT_ABS)
+     *
      * @ingroup CoreLibActivities
      */
     class RTT_API Activity
@@ -184,6 +189,8 @@ namespace RTT
 
         virtual bool setCpuAffinity(unsigned cpu);
 
+        void setWaitPeriodPolicy(int p);
+
         virtual os::ThreadInterface* thread();
 
         /**
@@ -225,6 +232,7 @@ namespace RTT
          */
         bool mtimeout;
         bool mstopRequested;
+        bool mabswaitpolicy;
     };
 
 }

rtt/os/ThreadInterface.hpp

@@ -196,7 +196,8 @@ namespace RTT
 
             /**
              * Set the wait policy of a periodic thread
-             * @param The wait policy between ORO_WAIT_ABS (absolute wait) and ORO_WAIT_REL (relative wait)
+             * @param The wait policy can be ORO_WAIT_ABS (absolute wait)
+             * and ORO_WAIT_REL (relative wait, with respect to current time)
              */
             virtual void setWaitPeriodPolicy(int p) = 0;