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 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;
}
/**
* Lock this mutex, but don't wait longer for the lock
* than the specified timeout.
*
* @param s The maximum time to wait before aqcuiring the lock.
* @return true when the locking succeeded, false otherwise
*/
virtual bool timedlock(Seconds s)
{
if ( rtos_mutex_lock_until( &m, rtos_get_time_ns() + Seconds_to_nsecs(s) ) == 0 )
return true;
return false;
}
INTERNAL_QUAL void rtos_task_set_period( RTOS_TASK* mytask, NANO_TIME nanosecs )
{
mytask->period = nanosecs;
mytask->periodMark = rtos_get_time_ns() + nanosecs;
}
