author | Mongo |
Tue, 13 Dec 2011 17:38:12 +0100 | |
changeset 671 | 06b30f679531 |
parent 468 | 787a54d068d6 |
child 801 | 32d146b64a35 |
permissions | -rwxr-xr-x |
454 | 1 |
#include <stdlib.h> |
2 |
#include <unistd.h> |
|
3 |
#include <sys/mman.h> |
|
4 |
||
5 |
#include <sys/poll.h> |
|
6 |
#include <rtai_lxrt.h> |
|
7 |
||
8 |
#include <rtai_sem.h> |
|
9 |
#include <pthread.h> |
|
10 |
#include <errno.h> |
|
11 |
||
12 |
#include "applicfg.h" |
|
13 |
#include "can_driver.h" |
|
14 |
#include "timer.h" |
|
15 |
||
16 |
#define TIMERLOOP_TASK_CREATED 1 |
|
17 |
||
18 |
TimerCallback_t exitall; |
|
19 |
||
20 |
SEM *CanFestival_mutex; |
|
21 |
SEM *condition_mutex; |
|
22 |
SEM *control_task; |
|
23 |
CND *timer_set; |
|
24 |
||
25 |
// realtime task structures |
|
26 |
RT_TASK *timerloop_task; |
|
27 |
RT_TASK *Main_Task; |
|
28 |
||
29 |
// linux threads id's |
|
30 |
static pthread_t timerloop_thr; |
|
31 |
||
32 |
RTIME last_time_read; |
|
33 |
RTIME last_occured_alarm; |
|
34 |
RTIME last_timeout_set; |
|
35 |
||
36 |
int stop_timer = 0; |
|
37 |
||
38 |
void TimerInit(void) |
|
39 |
{ |
|
40 |
/* Init Main Task */ |
|
41 |
if (!(Main_Task = rt_thread_init(rt_get_name(0), 0, 0, SCHED_FIFO, 1))) { |
|
42 |
printf("CANNOT INIT MAIN TASK\n"); |
|
43 |
exit(1); |
|
44 |
} |
|
45 |
||
46 |
/* Init Mutex */ |
|
47 |
CanFestival_mutex = rt_sem_init(rt_get_name(0), 1); |
|
48 |
condition_mutex = rt_typed_sem_init(rt_get_name(0), 1, RES_SEM); |
|
49 |
timer_set = rt_cond_init(rt_get_name(0)); |
|
50 |
control_task = rt_sem_init(rt_get_name(0), 0); |
|
51 |
/* Set timer mode and start timer */ |
|
52 |
rt_set_oneshot_mode(); |
|
53 |
start_rt_timer(0); |
|
54 |
} |
|
55 |
||
56 |
/** |
|
57 |
* Stop Timer Task |
|
58 |
* @param exitfunction |
|
59 |
*/ |
|
60 |
void StopTimerLoop(TimerCallback_t exitfunction) |
|
61 |
{ |
|
62 |
exitall = exitfunction; |
|
63 |
stop_timer = 1; |
|
64 |
rt_cond_signal(timer_set); |
|
65 |
} |
|
66 |
||
67 |
/** |
|
68 |
* Clean all Semaphores and main task |
|
69 |
*/ |
|
70 |
void TimerCleanup(void) |
|
71 |
{ |
|
72 |
/* Stop timer */ |
|
73 |
stop_rt_timer(); |
|
74 |
||
75 |
/* Delete all mutex and the main task */ |
|
76 |
rt_sem_delete(CanFestival_mutex); |
|
77 |
rt_sem_delete(condition_mutex); |
|
78 |
rt_sem_delete(timer_set); |
|
79 |
rt_sem_delete(control_task); |
|
80 |
rt_thread_delete(Main_Task); |
|
81 |
} |
|
82 |
||
83 |
/** |
|
84 |
* Take a semaphore |
|
85 |
*/ |
|
86 |
void EnterMutex(void) |
|
87 |
{ |
|
88 |
rt_sem_wait(CanFestival_mutex); |
|
89 |
} |
|
90 |
||
91 |
/** |
|
92 |
* Signaling a semaphore |
|
93 |
*/ |
|
94 |
void LeaveMutex(void) |
|
95 |
{ |
|
96 |
rt_sem_signal(CanFestival_mutex); |
|
97 |
} |
|
98 |
||
99 |
static TimerCallback_t init_callback; |
|
100 |
||
101 |
/** |
|
102 |
* Timer Task |
|
103 |
*/ |
|
104 |
void timerloop_task_proc(void *arg) |
|
105 |
{ |
|
106 |
int ret = 0; |
|
107 |
// lock process in to RAM |
|
108 |
mlockall(MCL_CURRENT | MCL_FUTURE); |
|
109 |
timerloop_task = rt_thread_init(rt_get_name(0), 0, 0, SCHED_FIFO, 1); |
|
110 |
rt_make_hard_real_time(); |
|
111 |
||
112 |
getElapsedTime(); |
|
113 |
last_timeout_set = 0; |
|
114 |
last_occured_alarm = last_time_read; |
|
115 |
||
116 |
/* trigger first alarm */ |
|
117 |
SetAlarm(NULL, 0, init_callback, 0, 0); |
|
118 |
||
119 |
do{ |
|
120 |
RTIME real_alarm; |
|
121 |
rt_sem_wait(condition_mutex); |
|
122 |
if(last_timeout_set == TIMEVAL_MAX) |
|
123 |
{ |
|
124 |
ret = rt_cond_wait( |
|
125 |
timer_set, |
|
126 |
condition_mutex); /* Then sleep until next message*/ |
|
127 |
||
128 |
rt_sem_signal(condition_mutex); |
|
129 |
}else{ |
|
130 |
real_alarm = last_time_read + last_timeout_set; |
|
131 |
ret = rt_cond_wait_until( |
|
132 |
timer_set, |
|
133 |
condition_mutex, |
|
134 |
real_alarm); /* Else, sleep until next deadline */ |
|
135 |
if(ret == SEM_TIMOUT){ |
|
136 |
last_occured_alarm = real_alarm; |
|
137 |
rt_sem_signal(condition_mutex); |
|
138 |
EnterMutex(); |
|
139 |
TimeDispatch(); |
|
140 |
LeaveMutex(); |
|
141 |
}else{ |
|
142 |
rt_sem_signal(condition_mutex); |
|
143 |
} |
|
144 |
} |
|
145 |
}while ( ret != SEM_ERR && !stop_timer); |
|
146 |
if(exitall){ |
|
147 |
EnterMutex(); |
|
148 |
exitall(NULL,0); |
|
149 |
LeaveMutex(); |
|
150 |
} |
|
151 |
rt_make_soft_real_time(); |
|
152 |
rt_thread_delete(timerloop_task); |
|
153 |
} |
|
154 |
||
155 |
/** |
|
156 |
* Create the Timer Task |
|
157 |
* @param _init_callback |
|
158 |
*/ |
|
159 |
void StartTimerLoop(TimerCallback_t _init_callback) |
|
160 |
{ |
|
161 |
stop_timer = 0; |
|
162 |
init_callback = _init_callback; |
|
163 |
||
164 |
/* start timerloop_task ( do nothing and get blocked ) */ |
|
165 |
timerloop_thr = rt_thread_create(timerloop_task_proc, NULL, 0); |
|
166 |
} |
|
167 |
||
168 |
/* We assume that ReceiveLoop_task_proc is always the same */ |
|
169 |
static void (*rtai_ReceiveLoop_task_proc)(CAN_PORT) = NULL; |
|
170 |
||
171 |
/** |
|
172 |
* Enter in realtime and start the CAN receiver loop |
|
173 |
* @param port |
|
174 |
*/ |
|
175 |
void rtai_canReceiveLoop(CAN_PORT port) |
|
176 |
{ |
|
177 |
RT_TASK *current_task; |
|
178 |
mlockall(MCL_CURRENT | MCL_FUTURE); |
|
179 |
current_task = rt_thread_init(rt_get_name(0), 0, 0, SCHED_FIFO, 1); |
|
180 |
rt_make_hard_real_time(); |
|
181 |
||
182 |
rt_sem_signal(control_task); |
|
183 |
||
184 |
/* Call original receive loop with port struct as a param */ |
|
185 |
rtai_ReceiveLoop_task_proc(port); |
|
186 |
||
187 |
rt_make_soft_real_time(); |
|
188 |
rt_thread_delete(current_task); |
|
189 |
} |
|
190 |
||
191 |
/** |
|
192 |
* Create the CAN Receiver Task |
|
193 |
* @param fd0 CAN port |
|
194 |
* @param *ReceiveLoop_thread CAN receiver thread |
|
195 |
* @param *ReceiveLoop_task_proc CAN receiver task |
|
196 |
*/ |
|
197 |
void CreateReceiveTask(CAN_PORT fd0, TASK_HANDLE *ReceiveLoop_thread, void* ReceiveLoop_task_proc) |
|
198 |
{ |
|
199 |
rtai_ReceiveLoop_task_proc = ReceiveLoop_task_proc; |
|
200 |
*ReceiveLoop_thread = rt_thread_create(rtai_canReceiveLoop, (void*)fd0, 0); |
|
201 |
rt_sem_wait(control_task); |
|
202 |
} |
|
203 |
||
204 |
/** |
|
205 |
* Wait for the CAN Receiver Task end |
|
206 |
* @param *ReceiveLoop_thread CAN receiver thread |
|
207 |
*/ |
|
208 |
void WaitReceiveTaskEnd(TASK_HANDLE *ReceiveLoop_thread) |
|
209 |
{ |
|
210 |
rt_thread_join(*ReceiveLoop_thread); |
|
211 |
} |
|
212 |
||
213 |
/** |
|
214 |
* Set timer for the next wakeup |
|
215 |
* @param value |
|
216 |
*/ |
|
217 |
void setTimer(TIMEVAL value) |
|
218 |
{ |
|
219 |
rt_sem_wait(condition_mutex); |
|
468
787a54d068d6
Fixed minor typos in timers_rtai.c and timers_xeno.c
etisserant
parents:
454
diff
changeset
|
220 |
last_timeout_set = value; |
454 | 221 |
rt_sem_signal(condition_mutex); |
222 |
rt_cond_signal(timer_set); |
|
223 |
} |
|
224 |
||
225 |
/** |
|
226 |
* Get the elapsed time since the last alarm |
|
227 |
* @return a time in nanoseconds |
|
228 |
*/ |
|
229 |
TIMEVAL getElapsedTime(void) |
|
230 |
{ |
|
231 |
RTIME res; |
|
232 |
rt_sem_wait(condition_mutex); |
|
233 |
last_time_read = rt_get_time(); |
|
234 |
res = last_time_read - last_occured_alarm; |
|
235 |
rt_sem_signal(condition_mutex); |
|
236 |
return res; |
|
237 |
} |