1 #include <stdlib.h> |
1 #include <stdlib.h> |
2 |
2 |
3 #include <sys/time.h> |
3 #include <sys/time.h> |
4 #include <pthread.h> |
4 #include <pthread.h> |
5 #include <signal.h> |
5 #include <signal.h> |
|
6 #include <time.h> |
6 |
7 |
7 #include "applicfg.h" |
8 #include "applicfg.h" |
8 #include "timer.h" |
9 #include "timer.h" |
9 |
10 |
10 pthread_mutex_t CanFestival_mutex = PTHREAD_MUTEX_INITIALIZER; |
11 static pthread_mutex_t CanFestival_mutex = PTHREAD_MUTEX_INITIALIZER; |
11 |
12 |
12 TASK_HANDLE TimerLoopThread; |
13 static struct timeval last_sig; |
13 |
14 |
14 TIMEVAL last_time_set = TIMEVAL_MAX; |
15 static timer_t timer; |
15 |
|
16 struct timeval last_sig; |
|
17 |
|
18 timer_t timer; |
|
19 |
16 |
20 void TimerCleanup(void) |
17 void TimerCleanup(void) |
21 { |
18 { |
22 /* only used in realtime apps */ |
19 /* only used in realtime apps */ |
23 } |
20 } |
24 |
21 |
25 void EnterMutex(void) |
22 void EnterMutex(void) |
26 { |
23 { |
27 pthread_mutex_lock(&CanFestival_mutex); |
24 if(pthread_mutex_lock(&CanFestival_mutex)) { |
|
25 fprintf(stderr, "pthread_mutex_lock() failed\n"); |
|
26 } |
28 } |
27 } |
29 |
28 |
30 void LeaveMutex(void) |
29 void LeaveMutex(void) |
31 { |
30 { |
32 pthread_mutex_unlock(&CanFestival_mutex); |
31 if(pthread_mutex_unlock(&CanFestival_mutex)) { |
|
32 fprintf(stderr, "pthread_mutex_unlock() failed\n"); |
|
33 } |
33 } |
34 } |
34 |
35 |
35 void timer_notify(sigval_t val) |
36 void timer_notify(sigval_t val) |
36 { |
37 { |
37 gettimeofday(&last_sig,NULL); |
38 if(gettimeofday(&last_sig,NULL)) { |
|
39 perror("gettimeofday()"); |
|
40 } |
38 EnterMutex(); |
41 EnterMutex(); |
39 TimeDispatch(); |
42 TimeDispatch(); |
40 LeaveMutex(); |
43 LeaveMutex(); |
41 // printf("getCurrentTime() return=%u\n", p.tv_usec); |
44 // printf("getCurrentTime() return=%u\n", p.tv_usec); |
42 } |
45 } |
44 void TimerInit(void) |
47 void TimerInit(void) |
45 { |
48 { |
46 struct sigevent sigev; |
49 struct sigevent sigev; |
47 |
50 |
48 // Take first absolute time ref. |
51 // Take first absolute time ref. |
49 gettimeofday(&last_sig,NULL); |
52 if(gettimeofday(&last_sig,NULL)){ |
|
53 perror("gettimeofday()"); |
|
54 } |
50 |
55 |
51 #if defined(__UCLIBC__) |
56 #if defined(__UCLIBC__) |
52 int ret; |
57 int ret; |
53 ret = timer_create(CLOCK_PROCESS_CPUTIME_ID, NULL, &timer); |
58 ret = timer_create(CLOCK_PROCESS_CPUTIME_ID, NULL, &timer); |
54 signal(SIGALRM, timer_notify); |
59 signal(SIGALRM, timer_notify); |
57 sigev.sigev_value.sival_int = 0; |
62 sigev.sigev_value.sival_int = 0; |
58 sigev.sigev_notify = SIGEV_THREAD; |
63 sigev.sigev_notify = SIGEV_THREAD; |
59 sigev.sigev_notify_attributes = NULL; |
64 sigev.sigev_notify_attributes = NULL; |
60 sigev.sigev_notify_function = timer_notify; |
65 sigev.sigev_notify_function = timer_notify; |
61 |
66 |
62 timer_create (CLOCK_REALTIME, &sigev, &timer); |
67 if(timer_create (CLOCK_REALTIME, &sigev, &timer)) { |
|
68 perror("timer_create()"); |
|
69 } |
63 #endif |
70 #endif |
64 } |
71 } |
65 |
72 |
66 void StopTimerLoop(TimerCallback_t exitfunction) |
73 void StopTimerLoop(TimerCallback_t exitfunction) |
67 { |
74 { |
68 EnterMutex(); |
75 EnterMutex(); |
69 timer_delete (timer); |
76 if(timer_delete (timer)) { |
|
77 perror("timer_delete()"); |
|
78 } |
70 exitfunction(NULL,0); |
79 exitfunction(NULL,0); |
71 LeaveMutex(); |
80 LeaveMutex(); |
72 } |
81 } |
73 |
82 |
74 void StartTimerLoop(TimerCallback_t init_callback) |
83 void StartTimerLoop(TimerCallback_t init_callback) |
89 * Enter in realtime and start the CAN receiver loop |
98 * Enter in realtime and start the CAN receiver loop |
90 * @param port |
99 * @param port |
91 */ |
100 */ |
92 void* unixtimer_canReceiveLoop(void* port) |
101 void* unixtimer_canReceiveLoop(void* port) |
93 { |
102 { |
94 |
|
95 /*get signal*/ |
103 /*get signal*/ |
96 signal(SIGTERM, canReceiveLoop_signal); |
104 if(signal(SIGTERM, canReceiveLoop_signal) == SIG_ERR) { |
|
105 perror("signal()"); |
|
106 } |
97 unixtimer_ReceiveLoop_task_proc((CAN_PORT)port); |
107 unixtimer_ReceiveLoop_task_proc((CAN_PORT)port); |
|
108 |
|
109 return NULL; |
98 } |
110 } |
99 |
111 |
100 void CreateReceiveTask(CAN_PORT port, TASK_HANDLE* Thread, void* ReceiveLoopPtr) |
112 void CreateReceiveTask(CAN_PORT port, TASK_HANDLE* Thread, void* ReceiveLoopPtr) |
101 { |
113 { |
102 unixtimer_ReceiveLoop_task_proc = ReceiveLoopPtr; |
114 unixtimer_ReceiveLoop_task_proc = ReceiveLoopPtr; |
103 pthread_create(Thread, NULL, unixtimer_canReceiveLoop, (void*)port); |
115 if(pthread_create(Thread, NULL, unixtimer_canReceiveLoop, (void*)port)) { |
|
116 perror("pthread_create()"); |
|
117 } |
104 } |
118 } |
105 |
119 |
106 void WaitReceiveTaskEnd(TASK_HANDLE *Thread) |
120 void WaitReceiveTaskEnd(TASK_HANDLE *Thread) |
107 { |
121 { |
108 pthread_kill(*Thread, SIGTERM); |
122 if(pthread_kill(*Thread, SIGTERM)) { |
109 pthread_join(*Thread, NULL); |
123 perror("pthread_kill()"); |
|
124 } |
|
125 if(pthread_join(*Thread, NULL)) { |
|
126 perror("pthread_join()"); |
|
127 } |
110 } |
128 } |
111 |
129 |
112 #define maxval(a,b) ((a>b)?a:b) |
130 #define maxval(a,b) ((a>b)?a:b) |
113 void setTimer(TIMEVAL value) |
131 void setTimer(TIMEVAL value) |
114 { |
132 { |
126 } |
144 } |
127 |
145 |
128 TIMEVAL getElapsedTime(void) |
146 TIMEVAL getElapsedTime(void) |
129 { |
147 { |
130 struct timeval p; |
148 struct timeval p; |
131 gettimeofday(&p,NULL); |
149 if(gettimeofday(&p,NULL)) { |
|
150 perror("gettimeofday()"); |
|
151 } |
132 // printf("getCurrentTime() return=%u\n", p.tv_usec); |
152 // printf("getCurrentTime() return=%u\n", p.tv_usec); |
133 return (p.tv_sec - last_sig.tv_sec)* 1000000 + p.tv_usec - last_sig.tv_usec; |
153 return (p.tv_sec - last_sig.tv_sec)* 1000000 + p.tv_usec - last_sig.tv_usec; |
134 } |
154 } |