diff --git a/rtt/Activity.cpp b/rtt/Activity.cpp index effd66471..92e98e68b 100644 --- a/rtt/Activity.cpp +++ b/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), mstopRequested(false), mabswaitpolicy(false) + update_period(0.0), mtimeout(false), mstopRequested(false), mwaitpolicy(ORO_WAIT_ABS) { } 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), mstopRequested(false), mabswaitpolicy(false) + update_period(0.0), mtimeout(false), mstopRequested(false), mwaitpolicy(ORO_WAIT_ABS) { } 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), mstopRequested(false), mabswaitpolicy(false) + update_period(period), mtimeout(false), mstopRequested(false), mwaitpolicy(ORO_WAIT_ABS) { // 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 @@ -77,13 +77,13 @@ namespace RTT Activity::Activity(int scheduler, int priority, RunnableInterface* r, const std::string& name ) : ActivityInterface(r), os::Thread(scheduler, priority, 0.0, 0, name ), - update_period(0.0), mtimeout(false), mstopRequested(false), mabswaitpolicy(false) + update_period(0.0), mtimeout(false), mstopRequested(false), mwaitpolicy(ORO_WAIT_ABS) { } 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), mstopRequested(false), mabswaitpolicy(false) + update_period(period), mtimeout(false), mstopRequested(false), mwaitpolicy(ORO_WAIT_ABS) { // 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 @@ -93,7 +93,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), mstopRequested(false), mabswaitpolicy(false) + update_period(period), mtimeout(false), mstopRequested(false), mwaitpolicy(ORO_WAIT_ABS) { // 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 @@ -213,14 +213,17 @@ namespace RTT return; } else { // If periodic, sleep until wakeup time or a message comes in. - // when wakeup time passed, wait_until will return false and we recalculate wakeup + mtimeout + // when wakeup time passed, wait_until will return false and we recalculate wakeup + update_period bool time_elapsed = ! msg_cond.wait_until(msg_lock,wakeup); if (time_elapsed) { + nsecs now = os::TimeService::Instance()->getNSecs(); + + // calculate next wakeup point nsecs nsperiod = Seconds_to_nsecs(update_period); wakeup = wakeup + nsperiod; - // calculate next wakeup point, overruns causes skips: - nsecs now = os::TimeService::Instance()->getNSecs(); + + // detect overruns if ( wakeup < now ) { ++overruns; @@ -230,8 +233,10 @@ namespace RTT else if (overruns != 0) { --overruns; } - if ( !mabswaitpolicy && wakeup < now ) { - wakeup = wakeup + ((now-wakeup)/nsperiod+1)*nsperiod; // assumes that (now-wakeup)/nsperiod rounds down ! + + // ORO_WAIT_REL: reset next wakeup time to now (before step) + period + if ( mwaitpolicy == ORO_WAIT_REL ) { + wakeup = now + nsperiod; } mtimeout = true; } @@ -342,10 +347,7 @@ namespace RTT void Activity::setWaitPeriodPolicy(int p) { - if ( p == ORO_WAIT_ABS) - mabswaitpolicy = true; - else - mabswaitpolicy = false; + mwaitpolicy = p; } } diff --git a/rtt/Activity.hpp b/rtt/Activity.hpp index 96fa418fc..0a57abe6f 100644 --- a/rtt/Activity.hpp +++ b/rtt/Activity.hpp @@ -232,7 +232,7 @@ namespace RTT */ bool mtimeout; bool mstopRequested; - bool mabswaitpolicy; + int mwaitpolicy; }; } diff --git a/rtt/os/ecos/fosi.h b/rtt/os/ecos/fosi.h index 05b40823b..54a18ddeb 100644 --- a/rtt/os/ecos/fosi.h +++ b/rtt/os/ecos/fosi.h @@ -49,8 +49,8 @@ extern "C" #define ORO_SCHED_RT 0 #define ORO_SCHED_OTHER 0 -#define ORO_WAIT_ABS 0 /** Not supported for the ecos target */ -#define ORO_WAIT_REL 1 /** Not supported for the ecos target */ +#define ORO_WAIT_ABS 0 +#define ORO_WAIT_REL 1 typedef long long NANO_TIME; typedef cyg_tick_count_t TICK_TIME; diff --git a/rtt/os/xenomai/fosi.h b/rtt/os/xenomai/fosi.h index 7a8842891..0ed43e80d 100644 --- a/rtt/os/xenomai/fosi.h +++ b/rtt/os/xenomai/fosi.h @@ -119,8 +119,8 @@ extern "C" { #define ORO_SCHED_RT 0 /** Hard real-time */ #define ORO_SCHED_OTHER 1 /** Soft real-time */ -#define ORO_WAIT_ABS 0 /** Not supported for the xenomai target */ -#define ORO_WAIT_REL 1 /** Not supported for the xenomai target */ +#define ORO_WAIT_ABS 0 +#define ORO_WAIT_REL 1 // hrt is in ticks static inline TIME_SPEC ticks2timespec(TICK_TIME hrt) diff --git a/tests/tasks_test.cpp b/tests/tasks_test.cpp index a8f2e8eb4..892f30223 100644 --- a/tests/tasks_test.cpp +++ b/tests/tasks_test.cpp @@ -22,7 +22,9 @@ #include "tasks_test.hpp" #include +#include #include +#include #include #include @@ -35,23 +37,49 @@ using namespace boost; using namespace RTT; using namespace RTT::detail; -#define BOOST_CHECK_EQUAL_MESSAGE(M, v1, v2) BOOST_CHECK_MESSAGE( v1==v2, M) +#define BOOST_CHECK_EQUAL_MESSAGE(v1, v2, M) BOOST_CHECK_MESSAGE( v1==v2, M) +#define BOOST_REQUIRE_EQUAL_MESSAGE(v1, v2, M) BOOST_REQUIRE_MESSAGE( v1==v2, M) struct TestOverrun : public RunnableInterface { + nsecs sleep_time; bool fini; - bool initialize() { fini = false; return true; } + std::vector wakeup_time; // record wakeup times + std::vector additional_sleep_time; // sleep ... additional nsecs in cycle i + unsigned int cycle; + + TestOverrun() + : sleep_time(Seconds_to_nsecs(0.2)), fini(false), wakeup_time(3), additional_sleep_time(), cycle(0) + { + } + + bool initialize() { + sleep_time = Seconds_to_nsecs(0.2); + fini = false; + wakeup_time.assign(wakeup_time.size(), 0); + cycle = 0; + return true; + } void step() { - //requires that getPeriod() << 1 - usleep(200*1000); + if (cycle < wakeup_time.size()) + { + wakeup_time[cycle] = os::TimeService::Instance()->getNSecs(); + } + if (cycle < additional_sleep_time.size() && additional_sleep_time[cycle] > 0) { + usleep(additional_sleep_time[cycle]/1000); + } + ++cycle; + + //requires that getPeriod() << sleep_time + usleep(sleep_time/1000); // Tried to implement it like this for Xenomai, but the // underlying rt_task_sleep function always returns immediately // and returns zero (success). A plain usleep still works. #if 0 TIME_SPEC timevl; - timevl = ticks2timespec( nano2ticks(200*1000*1000) ); + timevl = ticks2timespec( nano2ticks(sleep_time) ); rtos_nanosleep( &timevl, 0); #endif } @@ -270,15 +298,13 @@ BOOST_AUTO_TEST_CASE( testFailInit ) } -#if !defined( OROCOS_TARGET_WIN32 ) && !defined(OROCOS_TARGET_LXRT) -BOOST_AUTO_TEST_CASE( testOverrun ) +BOOST_AUTO_TEST_CASE( testMaxOverrun ) { - bool r = false; // create boost::scoped_ptr run( new TestOverrun() ); boost::scoped_ptr t( new Activity(25, 0.1, 0,"ORThread") ); - //BOOST_CHECK_EQUAL(25,t->getPriority() ); - BOOST_CHECK_EQUAL(0.1,t->getPeriod() ); + BOOST_REQUIRE_EQUAL(0.1,t->getPeriod() ); + t->thread()->setMaxOverrun(1); t->run( run.get() ); @@ -294,16 +320,76 @@ BOOST_AUTO_TEST_CASE( testOverrun ) usleep(400*1000); Logger::log().setLogLevel(ll); - r = !t->isRunning(); + BOOST_REQUIRE_MESSAGE( !t->isRunning(), "Failed to detect step overrun in Thread"); + BOOST_CHECK_MESSAGE( run->fini, "Failed to execute finalize in emergencyStop" ); +} + +static const nsecs WaitPeriodPolicyTolerance = Seconds_to_nsecs(0.01); - t->run(0); +BOOST_AUTO_TEST_CASE( testDefaultWaitPeriodPolicy ) +{ + // create + boost::scoped_ptr run( new TestOverrun() ); + boost::scoped_ptr t( new Activity(25, 0.3, 0,"ORThread") ); + t->run( run.get() ); + run->additional_sleep_time.resize(1); + run->additional_sleep_time[0] = Seconds_to_nsecs(0.2); + nsecs period = Seconds_to_nsecs(t->getPeriod()); (void) period; - BOOST_REQUIRE_MESSAGE( r, "Failed to detect step overrun in Thread"); + // test default wait period policy (== ORO_WAIT_ABS) + BOOST_REQUIRE_MESSAGE( t->start(), "start thread"); + // In Xenomai (2.5), the first usleep returns immediately. + // We can 'fix' this by adding a log() statement before usleep() .... crap + usleep(100*1000); + usleep(1000*1000); + BOOST_REQUIRE_MESSAGE( t->stop(), "stop thread"); + BOOST_CHECK_SMALL( ((run->wakeup_time[1] - run->wakeup_time[0]) - Seconds_to_nsecs(0.4)), WaitPeriodPolicyTolerance ); // Second wakeup: 2 * sleep_time + BOOST_CHECK_SMALL( ((run->wakeup_time[2] - run->wakeup_time[1]) - Seconds_to_nsecs(0.2)), WaitPeriodPolicyTolerance ); // Third wakeup right after end of second step (ORO_WAIT_ABS) +} - BOOST_CHECK_MESSAGE( run->fini, "Failed to execute finalize in emergencyStop" ); +BOOST_AUTO_TEST_CASE( testAbsoluteWaitPeriodPolicy ) +{ + // create + boost::scoped_ptr run( new TestOverrun() ); + boost::scoped_ptr t( new Activity(25, 0.3, 0,"ORThread") ); + t->run( run.get() ); + run->additional_sleep_time.resize(1); + run->additional_sleep_time[0] = Seconds_to_nsecs(0.2); + nsecs period = Seconds_to_nsecs(t->getPeriod()); (void) period; + // test absolute wait period policy (ORO_WAIT_ABS) + t->setWaitPeriodPolicy(ORO_WAIT_ABS); + BOOST_REQUIRE_MESSAGE( t->start(), "start thread"); + // In Xenomai (2.5), the first usleep returns immediately. + // We can 'fix' this by adding a log() statement before usleep() .... crap + usleep(100*1000); + usleep(1000*1000); + BOOST_REQUIRE_MESSAGE( t->stop(), "stop thread"); + BOOST_CHECK_SMALL( ((run->wakeup_time[1] - run->wakeup_time[0]) - Seconds_to_nsecs(0.4)), WaitPeriodPolicyTolerance ); // Second wakeup: 2 * sleep_time (overrun) + BOOST_CHECK_SMALL( ((run->wakeup_time[2] - run->wakeup_time[1]) - Seconds_to_nsecs(0.2)), WaitPeriodPolicyTolerance ); // Third wakeup right after end of second step (ORO_WAIT_ABS) +} + +BOOST_AUTO_TEST_CASE( testRelativeWaitPeriodPolicy ) +{ + // create + boost::scoped_ptr run( new TestOverrun() ); + boost::scoped_ptr t( new Activity(25, 0.3, 0,"ORThread") ); + t->run( run.get() ); + run->additional_sleep_time.resize(1); + run->additional_sleep_time[0] = Seconds_to_nsecs(0.2); + nsecs period = Seconds_to_nsecs(t->getPeriod()); (void) period; + + // test relative wait period policy (ORO_WAIT_REL) + t->setWaitPeriodPolicy(ORO_WAIT_REL); + BOOST_REQUIRE_MESSAGE( t->start(), "start thread"); + // In Xenomai (2.5), the first usleep returns immediately. + // We can 'fix' this by adding a log() statement before usleep() .... crap + usleep(100*1000); + usleep(1000*1000); + BOOST_REQUIRE_MESSAGE( t->stop(), "stop thread"); + BOOST_CHECK_SMALL( ((run->wakeup_time[1] - run->wakeup_time[0]) - Seconds_to_nsecs(0.4)), WaitPeriodPolicyTolerance ); // Second wakeup: 2 * sleep_time (overrun) + BOOST_CHECK_SMALL( ((run->wakeup_time[2] - run->wakeup_time[1]) - period), WaitPeriodPolicyTolerance ); // Third wakeup after period (ORO_WAIT_REL) } -#endif BOOST_AUTO_TEST_CASE( testThread ) { @@ -320,9 +406,8 @@ BOOST_AUTO_TEST_CASE( testThread ) r = t->stop(); BOOST_CHECK_MESSAGE( r, "Failed to stop Thread" ); BOOST_CHECK_MESSAGE( run->stepped == true, "Step not executed" ); - BOOST_CHECK_EQUAL_MESSAGE("Periodic Failure: period of step() too long !", run->overfail, 0); - BOOST_CHECK_EQUAL_MESSAGE("Periodic Failure: period of step() too short!", run->underfail, 0); - t->run(0); + BOOST_CHECK_EQUAL_MESSAGE(run->overfail, 0, "Periodic Failure: period of step() too long !"); + BOOST_CHECK_EQUAL_MESSAGE(run->underfail, 0, "Periodic Failure: period of step() too short!"); } #if defined( OROCOS_TARGET_GNULINUX )