void workloop(void *t) {
Loop* me = (Loop*)t;
log_info("%s starting.", me->name);
LoopData loop;
loop.count = 0;
loop.period = start_period / n_worker;
loop.deadline = abs_start + loop.period*me->id;
// entering primary mode
rt_task_set_mode(0, T_WARNSW, NULL);/* Ask Xenomai to warn us upon
switches to secondary mode. */
RTIME now = rt_timer_read();
while(loop.deadline < now) loop.deadline += loop.period;
while(bTesting) {
rt_task_sleep_until(loop.deadline);//blocks /////////////////////
if(!bTesting) break;
now = rt_timer_read();
loop.jitter = now - loop.deadline;//measure jitter
/* Begin "work" ****************************************/
me->work(); //rt_task_sleep(100000000); //for debugging
/* End "work" ******************************************/
RTIME t0 = now;//use an easy to remember var
now = rt_timer_read();
// Post work book keeping ///////////////////////////////
//to report how much the work took
loop.t_work = now - t0;
if(me->loopdata_q.push(loop)) {
} else { /* Have to throw away data; need to alarm! */
log_alert("Loop data full");
}
if(!me->late_q.isEmpty()// Manage the late q
&& me->late_q[0].count < (loop.count - 100)) {
me->late_q.pop(); // if sufficiently old, forget about it
}
loop.deadline += loop.period;
if(now > loop.deadline) { // Did I miss the deadline?
// How badly did I miss the deadline?
// Definition of "badness": just a simple count over the past N loop
if(me->late_q.isFull()) { //FATAL
log_fatal("Missed too many deadlines");
break;
}
}
/* decrement the period by a fraction */
loop.period -= dec_ppm ? loop.period / (1000000 / dec_ppm) : 0;
if(loop.period < 1000000) break; /* Limit at 1 ms for now */
++loop.count;
}
rt_task_set_mode(T_WARNSW, 0, NULL);// popping out of primary mode
log_info("%s exiting.", me->name);
}
void makeOneBeep(int freqNr, int duration_in_seconds){
long ticks_between_toggle = rt_timer_ns2ticks((1000000000/2)/freq[freqNr]);
RTIME next_toggle_time = rt_timer_read();
RTIME stop_time = rt_timer_ns2ticks(duration_in_seconds*1000000000)+next_toggle_time;
while(next_toggle_time < stop_time){
next_toggle_time+=ticks_between_toggle;
rt_task_sleep_until(next_toggle_time);
toggleSpeaker();
}
}
void RT::OS::sleepTimestep(RT::OS::Task task)
{
xenomai_task_t *t = reinterpret_cast<xenomai_task_t *>(task);
// Prevent significant early wake up from happening and drying the Linux system
rt_task_sleep_until(t->next_t - t->wakeup_t);
// Busy sleep until ready for the next cycle
rt_timer_spin(rt_timer_ticks2ns(t->next_t - rt_timer_read()));
// Update next interrupt time
t->next_t += t->period;
}
int __po_hi_delay_until (const __po_hi_time_t* time)
{
#if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX)
pthread_mutex_t mutex;
pthread_cond_t cond;
struct timespec timer;
int ret;
timer.tv_sec = time->sec;
timer.tv_nsec = time->nsec;
if (pthread_mutex_init (&mutex, NULL) != 0)
{
__PO_HI_DEBUG_INFO ("[TIME] __po_hi_delay_until: cannot initialize mutex\n");
return (__PO_HI_ERROR_PTHREAD_MUTEX);
}
if (pthread_cond_init (&cond, NULL) != 0)
{
__PO_HI_DEBUG_INFO ("[TIME] __po_hi_delay_until: cannot initialize cond\n");
pthread_mutex_destroy (&mutex);
return (__PO_HI_ERROR_PTHREAD_COND);
}
pthread_mutex_lock (&mutex);
ret = pthread_cond_timedwait (&cond, &mutex, &timer);
if ( (ret != 0) && (ret != ETIMEDOUT))
{
__PO_HI_DEBUG_INFO ("[TIME] __po_hi_delay_until: delay until error\n");
ret = __PO_HI_ERROR_PTHREAD_COND;
}
else
{
ret = __PO_HI_SUCCESS;
}
pthread_mutex_unlock (&mutex);
if (pthread_cond_destroy (&cond) != 0)
{
ret = __PO_HI_ERROR_PTHREAD_COND;
}
if (pthread_mutex_destroy (&mutex) != 0)
{
ret = __PO_HI_ERROR_PTHREAD_MUTEX;
}
return (ret);
#elif defined (RTEMS_PURE)
return (__PO_HI_UNAVAILABLE);
#elif defined (XENO_NATIVE)
int ret;
ret = rt_task_sleep_until (rt_timer_ns2tsc ( (time->sec * 1000000000) + time->nsec));
if (ret)
{
__DEBUGMSG ("[TASK] Error in rt_task_sleep_until, ret=%d\n", ret);
return (__PO_HI_ERROR_PTHREAD_COND);
}
return (__PO_HI_SUCCESS);
#elif defined (_WIN32)
HANDLE hTimer = NULL;
LARGE_INTEGER ularge;
hTimer = CreateWaitableTimer(NULL, TRUE, NULL);
ularge = __po_hi_unix_seconds_to_windows_tick (time->sec, time->nsec);
if (!SetWaitableTimer(hTimer, &ularge, 0, NULL, NULL, 0))
{
__PO_HI_DEBUG_DEBUG("[DELAY UNTIL] SetWaitableTimer failed (%d)\n", GetLastError());
return 2;
}
if (WaitForSingleObject(hTimer, INFINITE) != WAIT_OBJECT_0)
{
__PO_HI_DEBUG_DEBUG("[DELAY UNTIL] WaitForSingleObject failed (%d)\n", GetLastError());
}
if (CloseHandle(hTimer) != TRUE)
{
__PO_HI_DEBUG_DEBUG("[DELAY UNTIL] CloseHandle failed (%d)\n", GetLastError());
}
return __PO_HI_SUCCESS;
#else
return (__PO_HI_UNAVAILABLE);
#endif
}
