Go to the documentation of this file.00001
00018 #include "RTPreemptEC.h"
00019 #include <stdlib.h>
00020 #include <stdio.h>
00021 #include <time.h>
00022 #include <sched.h>
00023 #include <sys/mman.h>
00024 #include <string.h>
00025 #include <rtm/ECFactory.h>
00026 #include <rtm/Manager.h>
00027 #include <coil/stringutil.h>
00028
00029 #define MAX_SAFE_STACK (8*1024)
00030 #define NSEC_PER_SEC 1000000000
00031
00032 namespace OpenRTM
00033 {
00041 RTPreemptEC::RTPreemptEC()
00042 : ::RTC::PeriodicExecutionContext(),
00043 m_priority(49), m_policy(SCHED_FIFO), m_waitoffset(0)
00044 {
00045 rtclog.setName("RTPreemptEC");
00046 coil::Properties& prop(::RTC::Manager::instance().getConfig());
00047
00048
00049 getProperty(prop, "exec_cxt.periodic.priority", m_priority);
00050 getProperty(prop, "exec_cxt.periodic.rtpreempt.priority", m_priority);
00051 RTC_DEBUG(("Priority: %d", m_priority));
00052
00053
00054 {
00055 std::string policy;
00056 getProperty(prop, "exec_cxt.periodic.rtpreempt.sched_policy", policy);
00057 if (!policy.empty())
00058 {
00059 coil::normalize(policy);
00060 if (policy == "rr") { m_policy = SCHED_RR; }
00061 if (policy == "fifo") { m_policy = SCHED_FIFO; }
00062 RTC_DEBUG(("Scheduling policy: %s", policy.c_str()));
00063 }
00064 else
00065 {
00066 RTC_DEBUG(("Scheduling policy: fifo"));
00067 }
00068 }
00069
00070
00071 getProperty(prop, "exec_cxt.periodic.rtpreempt.wait_offset", m_waitoffset);
00072 RTC_DEBUG(("Wait offset: %d [ns]", m_waitoffset));
00073
00074 }
00075
00083 RTPreemptEC::~RTPreemptEC()
00084 {
00085 }
00086
00094 int RTPreemptEC::svc(void)
00095 {
00096
00097 struct sched_param param;
00098 param.sched_priority = m_priority;
00099 if(sched_setscheduler(0, m_policy, ¶m) == -1)
00100 {
00101 std::cerr << "sched_setscheduler failed" << std::endl;
00102 return -1;
00103 }
00104
00105
00106 if(mlockall(MCL_CURRENT | MCL_FUTURE) == -1)
00107 {
00108 std::cerr << "mlockall failed" << std::endl;
00109 return -1;
00110 }
00111
00112
00113 unsigned char dummy[MAX_SAFE_STACK];
00114 memset(&dummy, 0, MAX_SAFE_STACK);
00115 struct timespec t0, t1, t;
00116 do
00117 {
00118 clock_gettime(CLOCK_MONOTONIC ,&t0);
00119 m_worker.mutex_.lock();
00120 while (!m_worker.running_)
00121 {
00122 m_worker.cond_.wait();
00123 }
00124 if (m_worker.running_)
00125 {
00126 std::for_each(m_comps.begin(), m_comps.end(), invoke_worker());
00127 }
00128 m_worker.mutex_.unlock();
00129 clock_gettime(CLOCK_MONOTONIC ,&t1);
00130
00131 if (!m_nowait)
00132 {
00133 if (t0.tv_nsec > t1.tv_nsec)
00134 {
00135 t.tv_nsec = m_period.usec() * 1000
00136 - (NSEC_PER_SEC - t0.tv_nsec + t1.tv_nsec) + m_waitoffset;
00137 t.tv_sec = m_period.sec()
00138 - (t1.tv_sec - 1 - t0.tv_sec);
00139 }
00140 else
00141 {
00142 t.tv_nsec = m_period.usec() * 1000
00143 - (t1.tv_nsec - t0.tv_nsec) + m_waitoffset;
00144 t.tv_sec = m_period.sec()
00145 - (t1.tv_sec - t0.tv_sec);
00146 }
00147
00148 if (t.tv_nsec < 0 || t.tv_sec < 0)
00149 {
00150 std::cerr << "faital error: deadline passed. " << std::endl;
00151 std::cerr << "Wait time: ";
00152 std::cerr << t.tv_sec << "[s], ";
00153 std::cerr << t.tv_nsec << "[ns]" << std::endl;
00154 std::cerr << "Next wait time force to: 0.0 [s]" << std::endl;
00155 continue;
00156 }
00157 clock_nanosleep(CLOCK_MONOTONIC, !TIMER_ABSTIME, &t, NULL);
00158 }
00159 } while (m_svc);
00160
00161 return 0;
00162 }
00163
00164 };
00165
00166
00167 extern "C"
00168 {
00176 void RTPreemptECInit(RTC::Manager* manager)
00177 {
00178 RTC::Manager::instance().
00179 registerECFactory("RTPreemptEC",
00180 RTC::ECCreate<OpenRTM::RTPreemptEC>,
00181 RTC::ECDelete<OpenRTM::RTPreemptEC>);
00182
00183 }
00184 };