479 /*****************************************************************************/ |
476 /*****************************************************************************/ |
480 |
477 |
481 /**********************************************************/ |
478 /**********************************************************/ |
482 void cleanup_all(void) |
479 void cleanup_all(void) |
483 { |
480 { |
484 printf("delete my_task\n"); |
481 run = 0; |
485 |
|
486 pthread_kill(cyclicthread, SIGHUP); |
|
487 pthread_join(cyclicthread, NULL); |
|
488 |
|
489 if (rt_fd >= 0) { |
|
490 printf("closing rt device %s\n", &rt_dev_file[0]); |
|
491 rt_dev_close(rt_fd); |
|
492 |
|
493 } |
|
494 } |
482 } |
495 |
483 |
496 void catch_signal(int sig) |
484 void catch_signal(int sig) |
497 { |
485 { |
498 cleanup_all(); |
486 cleanup_all(); |
502 |
490 |
503 |
491 |
504 |
492 |
505 void *my_thread(void *arg) |
493 void *my_thread(void *arg) |
506 { |
494 { |
507 struct sched_param param = { .sched_priority = 1 }; |
|
508 struct timespec next_period; |
495 struct timespec next_period; |
|
496 |
509 int counter = 0; |
497 int counter = 0; |
510 int divcounter = 0; |
498 int divcounter = 0; |
511 int divider = 10; |
499 int divider = 10; |
512 pthread_set_name_np(pthread_self(), "ec_xenomai_posix_test"); |
500 |
513 pthread_setschedparam(pthread_self(), SCHED_FIFO, ¶m); |
501 |
514 |
502 clock_gettime(CLOCK_REALTIME, &next_period); |
515 clock_gettime(CLOCK_MONOTONIC, &next_period); |
|
516 |
|
517 while(1) { |
503 while(1) { |
518 next_period.tv_nsec += cycle * 1000; |
504 next_period.tv_nsec += cycle * 1000; |
519 while (next_period.tv_nsec >= NSEC_PER_SEC) { |
505 while (next_period.tv_nsec >= NSEC_PER_SEC) { |
520 next_period.tv_nsec -= NSEC_PER_SEC; |
506 next_period.tv_nsec -= NSEC_PER_SEC; |
521 next_period.tv_sec++; |
507 next_period.tv_sec++; |
522 } |
508 } |
523 |
509 |
524 clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_period, NULL); |
510 clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &next_period, NULL); |
|
511 |
525 |
512 |
526 counter++; |
513 counter++; |
527 if (counter>600000) { |
514 if (counter>60000) { |
528 run=0; |
515 run=0; |
529 return NULL; |
516 return NULL; |
530 } |
517 } |
|
518 |
|
519 if(run == 0) { |
|
520 return NULL; |
|
521 } |
|
522 |
531 |
523 |
532 // receive ethercat |
524 // receive ethercat |
533 ecrt_rtdm_master_recieve(rt_fd); |
525 ecrt_rtdm_master_recieve(rt_fd); |
534 ecrt_rtdm_domain_process(rt_fd); |
526 ecrt_rtdm_domain_process(rt_fd); |
535 |
527 |
725 |
715 |
726 int ret; |
716 int ret; |
727 run=1; |
717 run=1; |
728 |
718 |
729 /* Create cyclic RT-thread */ |
719 /* Create cyclic RT-thread */ |
|
720 struct sched_param param = { .sched_priority = 82 }; |
|
721 pthread_attr_t thattr; |
730 pthread_attr_init(&thattr); |
722 pthread_attr_init(&thattr); |
731 pthread_attr_setdetachstate(&thattr, PTHREAD_CREATE_JOINABLE); |
723 pthread_attr_setdetachstate(&thattr, PTHREAD_CREATE_JOINABLE); |
732 pthread_attr_setstacksize(&thattr, PTHREAD_STACK_MIN); |
724 pthread_attr_setinheritsched(&thattr, PTHREAD_EXPLICIT_SCHED); |
|
725 pthread_attr_setschedpolicy(&thattr, SCHED_FIFO); |
|
726 pthread_setschedparam(cyclicthread, SCHED_FIFO, ¶m); |
|
727 pthread_set_name_np(cyclicthread, "ec_xenomai_posix_test"); |
733 ret = pthread_create(&cyclicthread, &thattr, &my_thread, NULL); |
728 ret = pthread_create(&cyclicthread, &thattr, &my_thread, NULL); |
734 if (ret) { |
729 if (ret) { |
735 fprintf(stderr, "%s: pthread_create cyclic task failed\n", |
730 fprintf(stderr, "%s: pthread_create cyclic task failed\n", |
736 strerror(-ret)); |
731 strerror(-ret)); |
737 goto failure; |
732 goto failure; |
738 } |
733 } |
739 pthread_setschedparam(pthread_self(), SCHED_FIFO, ¶m); |
734 |
740 |
735 |
741 |
736 |
742 |
737 |
743 while (run) |
738 while (run) |
744 { |
739 { |
745 sched_yield(); |
740 sched_yield(); |
746 } |
741 } |
747 |
742 |
748 //rt_task_delete(&my_task); |
743 |
749 |
744 |
750 |
745 |
751 |
|
752 pthread_kill(cyclicthread, SIGHUP); |
|
753 pthread_join(cyclicthread, NULL); |
|
754 |
746 |
755 |
747 |
756 if (rt_fd >= 0) |
748 if (rt_fd >= 0) |
757 { |
749 { |
758 printf("closing rt device %s\n", &rt_dev_file[0]); |
750 printf("closing rt device %s\n", &rt_dev_file[0]); |