15 #include <alchemy/task.h> |
15 #include <alchemy/task.h> |
16 #include <alchemy/timer.h> |
16 #include <alchemy/timer.h> |
17 #include <alchemy/sem.h> |
17 #include <alchemy/sem.h> |
18 #include <alchemy/pipe.h> |
18 #include <alchemy/pipe.h> |
19 |
19 |
|
20 #define _Log(level,text,...) \ |
|
21 {\ |
|
22 char mstr[256];\ |
|
23 snprintf(mstr, 255, text, ##__VA_ARGS__);\ |
|
24 LogMessage(level, mstr, strlen(mstr));\ |
|
25 } |
|
26 |
|
27 #define _LogError(text,...) _Log(LOG_CRITICAL, text, ##__VA_ARGS__) |
|
28 #define _LogWarning(text,...) _Log(LOG_WARNING, text, ##__VA_ARGS__) |
|
29 |
20 unsigned int PLC_state = 0; |
30 unsigned int PLC_state = 0; |
21 #define PLC_STATE_TASK_CREATED 1 |
31 #define PLC_STATE_TASK_CREATED 1 |
22 #define PLC_STATE_DEBUG_PIPE_CREATED 2 |
32 #define PLC_STATE_DEBUG_PIPE_CREATED 2 |
23 #define PLC_STATE_PYTHON_PIPE_CREATED 8 |
33 #define PLC_STATE_PYTHON_PIPE_CREATED 8 |
24 #define PLC_STATE_WAITDEBUG_PIPE_CREATED 16 |
34 #define PLC_STATE_WAITDEBUG_PIPE_CREATED 16 |
92 rt_task_set_periodic(&PLC_task, current_time + next, rt_timer_ns2ticks(period)); |
102 rt_task_set_periodic(&PLC_task, current_time + next, rt_timer_ns2ticks(period)); |
93 } |
103 } |
94 |
104 |
95 void PLC_task_proc(void *arg) |
105 void PLC_task_proc(void *arg) |
96 { |
106 { |
|
107 unsigned long overruns = 0; |
97 PLC_SetTimer(common_ticktime__, common_ticktime__); |
108 PLC_SetTimer(common_ticktime__, common_ticktime__); |
98 |
109 |
99 while (!PLC_shutdown) { |
110 while (!PLC_shutdown) { |
100 PLC_GetTime(&__CURRENT_TIME); |
111 PLC_GetTime(&__CURRENT_TIME); |
101 __run(); |
112 if(overruns == 0){ |
|
113 __run(); |
|
114 } else { |
|
115 // in case of overrun, don't run PLC on next cycle, to prevent CPU hogging. |
|
116 _LogWarning("PLC execution time is longer than requested PLC cyclic task interval. %d cycles skipped\n", overruns); |
|
117 // rt_printf("PLC execution time is longer than requested PLC cyclic task interval. %d cycles skipped\n", overruns); |
|
118 // increment tick count anyhow, so that task scheduling keeps consistent |
|
119 __tick += overruns; |
|
120 } |
102 if (PLC_shutdown) break; |
121 if (PLC_shutdown) break; |
103 rt_task_wait_period(NULL); |
122 rt_task_wait_period(&overruns); |
104 } |
123 } |
105 /* since xenomai 3 it is not enough to close() |
124 /* since xenomai 3 it is not enough to close() |
106 file descriptor to unblock read()... */ |
125 file descriptor to unblock read()... */ |
107 { |
126 { |
108 /* explicitely finish python thread */ |
127 /* explicitely finish python thread */ |
115 send_RT_to_nRT_signal(WaitDebug_handle, msg); |
134 send_RT_to_nRT_signal(WaitDebug_handle, msg); |
116 } |
135 } |
117 } |
136 } |
118 |
137 |
119 static unsigned long __debug_tick; |
138 static unsigned long __debug_tick; |
120 |
|
121 #define _Log(text, err) \ |
|
122 {\ |
|
123 char mstr[256];\ |
|
124 snprintf(mstr, 255, text " for %s (%d)", name, err);\ |
|
125 LogMessage(LOG_CRITICAL, mstr, strlen(mstr));\ |
|
126 } |
|
127 |
139 |
128 void *create_RT_to_nRT_signal(char* name){ |
140 void *create_RT_to_nRT_signal(char* name){ |
129 int new_index = -1; |
141 int new_index = -1; |
130 int ret; |
142 int ret; |
131 RT_to_nRT_signal_t *sig; |
143 RT_to_nRT_signal_t *sig; |
140 } |
152 } |
141 } |
153 } |
142 |
154 |
143 /* fail if none found */ |
155 /* fail if none found */ |
144 if(new_index == -1) { |
156 if(new_index == -1) { |
145 _Log("Maximum count of RT-PIPE reached while creating pipe", max_RT_to_nRT_signals); |
157 _LogError("Maximum count of RT-PIPE reached while creating pipe for %s (%d)", name, max_RT_to_nRT_signals); |
146 return NULL; |
158 return NULL; |
147 } |
159 } |
148 |
160 |
149 /* create rt pipe */ |
161 /* create rt pipe */ |
150 if(ret = rt_pipe_create(&sig->pipe, name, new_index, PIPE_SIZE) < 0){ |
162 if(ret = rt_pipe_create(&sig->pipe, name, new_index, PIPE_SIZE) < 0){ |
151 _Log("Failed opening real-time end of RT-PIPE", ret); |
163 _LogError("Failed opening real-time end of RT-PIPE for %s (%d)", name, ret); |
152 return NULL; |
164 return NULL; |
153 } |
165 } |
154 |
166 |
155 /* open pipe's userland */ |
167 /* open pipe's userland */ |
156 snprintf(pipe_dev, 63, "/dev/rtp%d", new_index); |
168 snprintf(pipe_dev, 63, "/dev/rtp%d", new_index); |
157 if((sig->pipe_fd = open(pipe_dev, O_RDWR)) == -1){ |
169 if((sig->pipe_fd = open(pipe_dev, O_RDWR)) == -1){ |
158 rt_pipe_delete(&sig->pipe); |
170 rt_pipe_delete(&sig->pipe); |
159 _Log("Failed opening non-real-time end of RT-PIPE", errno); |
171 _LogError("Failed opening non-real-time end of RT-PIPE for %s (%d)", name, errno); |
160 return NULL; |
172 return NULL; |
161 } |
173 } |
162 |
174 |
163 sig->used = 1; |
175 sig->used = 1; |
164 sig->name = name; |
176 sig->name = name; |
172 char *name = sig->name; |
184 char *name = sig->name; |
173 |
185 |
174 if(!sig->used) return; |
186 if(!sig->used) return; |
175 |
187 |
176 if(ret = rt_pipe_delete(&sig->pipe) != 0){ |
188 if(ret = rt_pipe_delete(&sig->pipe) != 0){ |
177 _Log("Failed closing real-time end of RT-PIPE", ret); |
189 _LogError("Failed closing real-time end of RT-PIPE for %s (%d)", name, ret); |
178 } |
190 } |
179 |
191 |
180 if(close(sig->pipe_fd) != 0){ |
192 if(close(sig->pipe_fd) != 0){ |
181 _Log("Failed closing non-real-time end of RT-PIPE", errno); |
193 _LogError("Failed closing non-real-time end of RT-PIPE for %s (%d)", name, errno); |
182 } |
194 } |
183 |
195 |
184 sig->used = 0; |
196 sig->used = 0; |
185 } |
197 } |
186 |
198 |