void CPCTimerHandler::run(){
//Calculate time handler thread period based on TICKS_PER_SECOND parameter configured in CMake
struct timespec stPeriod;
stPeriod.tv_sec = 0;
stPeriod.tv_nsec = (1000000000 / getTicksPerSecond());
if(stPeriod.tv_nsec < 1){
//Time base under 1 ns is not supported
DEVLOG_ERROR("TICKS_PER_SECOND greater than 1000000000 are not supported");
stPeriod.tv_nsec = 1;
}
//Make this thread periodic
unsigned long nOverRuns;
struct timespec stAuxTime;
clock_gettime(CLOCK_REALTIME, &stAuxTime);
stAuxTime.tv_nsec += 100000;
pthread_t oThreadId = pthread_self();
if(oThreadId != 0){
if(pthread_make_periodic_np(oThreadId,&stAuxTime,&stPeriod)!= 0){
DEVLOG_ERROR("Error could not set time handler thread period! %s\n", strerror(errno));
return;
}
}
while(isAlive()){
//TODO: Handle overruns
nextTick();
pthread_wait_np(&nOverRuns);
}
}
/* Periodic thread */
void *thread_square (void *dummy)
{
int err, cmd, m = 1;
struct timespec start, period, t, told;
/* Start a periodic task in 1s */
clock_gettime(CLOCK_REALTIME, &start);
start.tv_sec += 1;
period.tv_sec = 0;
period.tv_nsec = period_ns;
// Make task periodic
err = pthread_make_periodic_np(pthread_self(), &start, &period);
if (err)
{
fprintf(stderr,"failed to set periodic, code %d\n",err);
exit(EXIT_FAILURE);
}
/* Main loop */
for (;;)
{
unsigned long overruns = 0;
test_loops++;
/* Wait scheduler */
err = pthread_wait_np(&overruns);
if (err || overruns) {
fprintf(stderr,"wait_period failed: err %d, overruns: %lu\n", err, overruns);
exit(EXIT_FAILURE);
}
/* Write to GPIO */
cmd = (test_loops % (clic % 2 ? 4 : 2) ? 0 : 1);
if (rt_dev_ioctl(fd, cmd, 0) < 0)
fprintf (stderr, "rt_dev_ioctl error!\n");
/* old_time <-- current_time */
told.tv_sec = t.tv_sec;
told.tv_nsec = t.tv_nsec;
/* Get current time */
clock_gettime (CLOCK_REALTIME, &t);
/* Print if necessary */
if ((test_loops % loop_prt) == 0)
rt_printf ("Loop= %d dt= %d %d (%d ns)\n", test_loops, t.tv_sec - told.tv_sec, t.tv_nsec - told.tv_nsec, t.tv_nsec - told.tv_nsec - period_ns);
}
}
void *xenomai_timer_run_thread(rtdal_timer_t *obj) {
struct timespec period,start;
unsigned long overruns;
int s;
int n;
unsigned int tscnt=0;
struct timespec test;
rtdal_task_print_sched();
period.tv_sec=0;
period.tv_nsec=obj->period;
obj->stop = 0;
if (obj->wait_futex) {
futex_wait(obj->wait_futex);
obj->next.tv_sec+=3;
obj->next.tv_nsec=0;
} else {
clock_gettime(CLOCK_REALTIME, &obj->next);
obj->next.tv_sec++;
}
s=pthread_make_periodic_np(pthread_self(), &obj->next, &period);
if (s) {
printf("error %d\n",s);
return NULL;
}
printf("go!\n");
n=0;
while(!obj->stop) {
overruns=0;
if ((s=pthread_wait_np(&overruns))) {
if (s!=110)
printf("error2 %d\n",s);
}
if (overruns>0) {
printf("[%d] %d overrun\n",tscnt, overruns);
}
n++;
if (n>=obj->multiple) {
obj->period_function(obj->arg, &obj->next);
n=0;
}
}
s=0;
pthread_exit(&s);
return NULL;
}
void *xenomai_timer_run_thread(rtdal_timer_t *obj) {
struct timespec period;
unsigned long overruns;
int s;
int n;
unsigned int tscnt=0;
// pthread_set_mode_np(0, PTHREAD_WARNSW);
period.tv_sec=0;
period.tv_nsec=obj->period;
obj->stop = 0;
if (obj->wait_futex) {
futex_wait(obj->wait_futex);
obj->next.tv_sec+=2;
obj->next.tv_nsec=0;
} else {
clock_gettime(CLOCK_REALTIME, &obj->next);
obj->next.tv_sec+=2;
}
s=pthread_make_periodic_np(pthread_self(), &obj->next, &period);
if (s) {
printf("error %d\n",s);
return NULL;
}
n=0;
while(!obj->stop) {
overruns=0;
if ((s=pthread_wait_np(&overruns))) {
if (s!=110)
printf("error2 %d\n",s);
}
timelog(obj->log);
n++;
if (n>=obj->multiple) {
obj->period_function(obj->arg, &obj->next);
n=0;
}
}
s=0;
pthread_exit(&s);
return NULL;
}
void *thread_code(void *t) {
pthread_make_periodic_np (thread, gethrtime(), period);
do {
int n;
char buf[210];
pthread_wait_np();
n = rt_com_read(0, buf, sizeof(buf));
if (n > 0) {
buf[n] = 0;
rtl_printf("%s", buf);
}
rt_com_write(0, "test\n", 5);
} while (1);
return 0;
}
