RTPreemptEC.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
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     // Priority
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     // Policy
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     // Wait offset
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     // schedulaer setting
00097     struct sched_param param;
00098     param.sched_priority = m_priority;
00099     if(sched_setscheduler(0, m_policy, &param) == -1)
00100       {
00101         std::cerr << "sched_setscheduler failed" << std::endl;
00102         return -1;
00103       }
00104 
00105     // memory locking
00106     if(mlockall(MCL_CURRENT | MCL_FUTURE) == -1)
00107       {
00108         std::cerr << "mlockall failed" << std::endl;
00109         return -1;
00110       }
00111 
00112     // stack preallocation
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 };


openrtm_aist
Author(s): Noriaki Ando
autogenerated on Sun Mar 26 2017 03:37:17