INTERNAL_QUAL void rtos_task_make_periodic(RTOS_TASK* mytask, NANO_TIME nanosecs )
{
// set period
mytask->period = nanosecs;
// set next wake-up time.
mytask->periodMark = ticks2timespec( nano2ticks( rtos_get_time_ns() + nanosecs ) );
}
INTERNAL_QUAL int rtos_task_wait_period( RTOS_TASK* task )
{
if ( task->period == 0 )
return 0;
// record this to detect overrun.
NANO_TIME now = rtos_get_time_ns();
NANO_TIME wake= task->periodMark.tv_sec * 1000000000LL + task->periodMark.tv_nsec;
// inspired by nanosleep man page for this construct:
while ( clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &(task->periodMark), NULL) != 0 && errno == EINTR ) {
errno = 0;
}
if (task->wait_policy == ORO_WAIT_ABS)
{
// in the case of overrun by more than 4 periods,
// skip all the updates before now, with the next update aligned to period
int maxDelayInPeriods = 4;
NANO_TIME period = task->period;
if (now - wake > maxDelayInPeriods*period) {
period = period * ((now - wake) / period);
}
// program next period:
// 1. convert period to timespec
TIME_SPEC ts = ticks2timespec( nano2ticks(period) );
// 2. Add ts to periodMark (danger: tn guards for overflows!)
NANO_TIME tn = (task->periodMark.tv_nsec + ts.tv_nsec);
task->periodMark.tv_nsec = tn % 1000000000LL;
task->periodMark.tv_sec += ts.tv_sec + tn / 1000000000LL;
}
else
{
TIME_SPEC ts = ticks2timespec( nano2ticks( task->period) );
TIME_SPEC now = ticks2timespec( rtos_get_time_ns() );
NANO_TIME tn = (now.tv_nsec + ts.tv_nsec);
task->periodMark.tv_nsec = tn % 1000000000LL;
task->periodMark.tv_sec = ts.tv_sec + now.tv_sec + tn / 1000000000LL;
}
return now > wake ? -1 : 0;
}
INTERNAL_QUAL void rtos_task_yield(RTOS_TASK* t) {
#if 0
//under plain gnulinux, sched_yield may have little influence, so sleep
// to force rescheduling of other threads.
NANO_TIME timeRemaining = 1000; // 1ms
TIME_SPEC ts( ticks2timespec( timeRemaining ) );
rtos_nanosleep( &ts , NULL );
#else
int ret = sched_yield();
if ( ret != 0)
perror("rtos_task_yield");
#endif
}
INTERNAL_QUAL int rtos_task_wait_period( RTOS_TASK* task )
{
if ( task->period == 0 )
return 0;
//rtos_printf("Time is %lld nsec, Mark is %lld nsec.\n",rtos_get_time_ns(), task->periodMark );
// CALCULATE in nsecs
NANO_TIME timeRemaining = task->periodMark - rtos_get_time_ns();
// next wake-up time :
task->periodMark += task->period;
if ( timeRemaining > 0 ) {
//rtos_printf("Waiting for %lld nsec\n",timeRemaining);
TIME_SPEC ts( ticks2timespec( timeRemaining ) );
rtos_nanosleep( &ts , NULL );
return 0;
}
return -1;
}
