40 #include "../Time.hpp" 42 #include "../Logger.hpp" 46 #include "../rtt-config.h" 47 #include "../internal/CatchConfig.hpp" 49 #ifdef OROPKG_OS_THREAD_SCOPE 50 # include "../extras/dev/DigitalOutInterface.hpp" 51 #define SCOPE_INIT(name) 52 #define SCOPE_ON if ( task->d ) task->d->switchOn( bit ); 53 #define SCOPE_OFF if ( task->d ) task->d->switchOff( bit ); 54 #elif defined(HAVE_LTTNG_UST) && defined(OROPKG_OS_GNULINUX) 56 #define SCOPE_INIT(name) tracepoint(orocos_rtt, thread_init , name); 57 #define SCOPE_ON tracepoint(orocos_rtt, thread_scope, 1); 58 #define SCOPE_OFF tracepoint(orocos_rtt, thread_scope, 0); 60 #define SCOPE_INIT(name) 99 #ifdef OROPKG_OS_THREAD_SCOPE 119 if (task->period != 0) {
122 rtos_task_set_period(task->getTask(), 0);
153 if ( cur_period != task->
period) {
156 cur_period = task->
period;
175 else if (overruns != 0)
206 <<
" got too many periodic overruns in step() (" 207 << overruns <<
" times), stopped Thread !" 209 log() <<
" See Thread::setMaxOverrun() for info." 212 )
CATCH(std::exception
const& e,
217 <<
" caught a C++ exception, stopped thread !" 227 <<
" caught an unknown C++ exception, stopped thread !" 246 Seconds periods,
unsigned cpu_affinity,
const std::string & name) :
249 maxOverRun(OROSEM_OS_PERIODIC_THREADS_MAX_OVERRUN),
251 #ifdef OROPKG_OS_THREAD_SCOPE
256 this->
setup(_priority, cpu_affinity, name);
259 void Thread::setup(
int _priority,
unsigned cpu_affinity,
const std::string& name)
268 <<
", priority=" << _priority
269 <<
", CPU affinity=" << cpu_affinity
270 <<
", with name='" << name <<
"'" 276 <<
"Could not allocate configuration semaphore 'sem' for " 278 <<
". Throwing std::bad_alloc." <<
endlog();
281 throw std::bad_alloc();
287 #ifdef OROPKG_OS_THREAD_SCOPE 297 log(
Warning) <<
"Failed to find 'ThreadScope' object in DigitalOutInterface::nameserver." <<
endlog();
310 throw std::bad_alloc();
319 const char* modname =
getName();
321 log(
Info) <<
"Thread created with scheduler type '" 325 #ifdef OROPKG_OS_THREAD_SCOPE 329 log(
Info) <<
"ThreadScope :"<< modname <<
" toggles bit "<< bit <<
endlog();
351 #ifndef OROPKG_OS_MACOSX 389 if (result ==
false ||
active ==
false)
402 <<
"Thread::start(): sem_signal returns " << ret
444 log(
Error) <<
"Failed to stop thread " << this->
getName() <<
": breakLoop() returned true, but loop() function did not return after " <<
getStopTimeout() <<
" seconds."<<
endlog();
486 log(
Info) <<
"Setting scheduler type for Thread '" 488 << sched_type <<
endlog();
551 nsecs nsperiod = ns + 1000* 1000* 1000* s ;
555 if ( (nsperiod == 0 &&
period != 0) || (nsperiod != 0 &&
period == 0)) {
574 return this->
setPeriod( p.tv_sec, p.tv_nsec );
580 ns =
period - s*1000*1000*1000;
void setup(int _priority, unsigned cpu_affinity, const std::string &name)
int rtos_task_get_priority(const RTOS_TASK *task)
virtual void setMaxOverrun(int m)
static unsigned int default_stack_size
INTERNAL_QUAL void rtos_task_make_periodic(RTOS_TASK *mytask, NANO_TIME nanosecs)
#define TRY(C)
Contains static global configuration variables and cached entries.
static void setLockTimeoutNoPeriod(double timeout_in_s)
static NameServer< DigitalOutInterface * > nameserver
bool CheckScheduler(int &sched_type)
unsigned int rtos_task_get_pid(const RTOS_TASK *task)
Seconds nsecs_to_Seconds(const nsecs ns)
void setStopTimeout(Seconds s)
int rtos_sem_signal(rt_sem_t *m)
int rtos_task_set_scheduler(RTOS_TASK *t, int sched_type)
friend void * thread_function(void *t)
virtual bool setCpuAffinity(unsigned cpu_affinity)
int rtos_task_set_cpu_affinity(RTOS_TASK *task, unsigned cpu_affinity)
virtual bool setScheduler(int sched_type)
unsigned rtos_task_get_cpu_affinity(const RTOS_TASK *task)
virtual bool setPriority(int priority)
virtual RTOS_TASK * getTask()
static double lock_timeout_period_factor
virtual int getMaxOverrun() const
void rtos_task_set_wait_period_policy(RTOS_TASK *task, int policy)
static void setLockTimeoutPeriodFactor(double factor)
void rtos_task_delete(RTOS_TASK *mytask)
virtual bool isActive() const
INTERNAL_QUAL void rtos_task_yield(RTOS_TASK *)
int rtos_task_set_priority(RTOS_TASK *task, int priority)
unsigned int threadNumber() const
int rtos_sem_destroy(rt_sem_t *m)
virtual bool initialize()
const char * rtos_task_get_name(const RTOS_TASK *task)
A MutexTimedLock locks a Mutex object on construction and if successful, unlocks it on destruction of...
virtual unsigned getCpuAffinity() const
int rtos_task_wait_period(RTOS_TASK *task)
ValueType getObject(const NameType &s) const
Get the object registered for a name.
static double lock_timeout_no_period_in_s
virtual int getScheduler() const
bool setPeriod(Seconds s)
int rtos_sem_init(rt_sem_t *m, int value)
nsecs Seconds_to_nsecs(const Seconds s)
static void setStackSize(unsigned int ssize)
Thread(int scheduler, int priority, double period, unsigned cpu_affinity, const std::string &name)
int rtos_sem_value(rt_sem_t *m)
Seconds getStopTimeout() const
virtual void setWaitPeriodPolicy(int p)
Contains TaskContext, Activity, OperationCaller, Operation, Property, InputPort, OutputPort, Attribute.
void rtos_task_set_period(RTOS_TASK *mytask, NANO_TIME nanosecs)
virtual int getPriority() const
virtual Seconds getPeriod() const
struct timespec TIME_SPEC
virtual unsigned int getPid() const
int rtos_sem_wait(rt_sem_t *m)
virtual bool isPeriodic() const
static Logger::LogFunction endlog()
virtual nsecs getPeriodNS() const
virtual const char * getName() const
INTERNAL_QUAL int rtos_task_create(RTOS_TASK *task, int priority, unsigned cpu_affinity, const char *name, int sched_type, size_t stack_size, void *(*start_routine)(void *), ThreadInterface *obj)
virtual bool isRunning() const
MutexLock is a scope based Monitor, protecting critical sections with a Mutex object through locking ...
int rtos_task_get_scheduler(const RTOS_TASK *t)