|
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);
|
|
|
220 |
last_timeout_set = (value == TIMEVAL_MAX) ? TIMEVAL_MAX : value;
|
|
|
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 |
}
|