33 #include <string.h> |
33 #include <string.h> |
34 #include <sys/resource.h> |
34 #include <sys/resource.h> |
35 #include <sys/time.h> |
35 #include <sys/time.h> |
36 #include <sys/types.h> |
36 #include <sys/types.h> |
37 #include <unistd.h> |
37 #include <unistd.h> |
|
38 #include <time.h> /* clock_gettime() */ |
|
39 #include <sys/mman.h> /* mlockall() */ |
38 |
40 |
39 /****************************************************************************/ |
41 /****************************************************************************/ |
40 |
42 |
41 #include "ecrt.h" |
43 #include "ecrt.h" |
42 |
44 |
43 /****************************************************************************/ |
45 /****************************************************************************/ |
44 |
46 |
45 // Application parameters |
47 /** Task period in ns. */ |
46 #define FREQUENCY 100 |
48 #define PERIOD_NS (1000000) |
47 #define PRIORITY 1 |
49 |
48 |
50 #define MAX_SAFE_STACK (8 * 1024) /* The maximum stack size which is |
49 // Optional features |
51 guranteed safe to access without |
50 #define CONFIGURE_PDOS 1 |
52 faulting */ |
51 #define SDO_ACCESS 0 |
53 |
|
54 /****************************************************************************/ |
|
55 |
|
56 /* Constants */ |
|
57 #define NSEC_PER_SEC (1000000000) |
|
58 #define FREQUENCY (NSEC_PER_SEC / PERIOD_NS) |
52 |
59 |
53 /****************************************************************************/ |
60 /****************************************************************************/ |
54 |
61 |
55 // EtherCAT |
62 // EtherCAT |
56 static ec_master_t *master = NULL; |
63 static ec_master_t *master = NULL; |
59 static ec_domain_t *domain1 = NULL; |
66 static ec_domain_t *domain1 = NULL; |
60 static ec_domain_state_t domain1_state = {}; |
67 static ec_domain_state_t domain1_state = {}; |
61 |
68 |
62 static ec_slave_config_t *sc_ana_in = NULL; |
69 static ec_slave_config_t *sc_ana_in = NULL; |
63 static ec_slave_config_state_t sc_ana_in_state = {}; |
70 static ec_slave_config_state_t sc_ana_in_state = {}; |
64 |
|
65 // Timer |
|
66 static unsigned int sig_alarms = 0; |
|
67 static unsigned int user_alarms = 0; |
|
68 |
71 |
69 /****************************************************************************/ |
72 /****************************************************************************/ |
70 |
73 |
71 // process data |
74 // process data |
72 static uint8_t *domain1_pd = NULL; |
75 static uint8_t *domain1_pd = NULL; |
99 |
102 |
100 static unsigned int counter = 0; |
103 static unsigned int counter = 0; |
101 static unsigned int blink = 0; |
104 static unsigned int blink = 0; |
102 |
105 |
103 /*****************************************************************************/ |
106 /*****************************************************************************/ |
104 |
|
105 #if CONFIGURE_PDOS |
|
106 |
107 |
107 // Analog in -------------------------- |
108 // Analog in -------------------------- |
108 |
109 |
109 static ec_pdo_entry_info_t el3102_pdo_entries[] = { |
110 static ec_pdo_entry_info_t el3102_pdo_entries[] = { |
110 {0x3101, 1, 8}, // channel 1 status |
111 {0x3101, 1, 8}, // channel 1 status |
163 static ec_sync_info_t el2004_syncs[] = { |
164 static ec_sync_info_t el2004_syncs[] = { |
164 {0, EC_DIR_OUTPUT, 4, el2004_pdos}, |
165 {0, EC_DIR_OUTPUT, 4, el2004_pdos}, |
165 {1, EC_DIR_INPUT}, |
166 {1, EC_DIR_INPUT}, |
166 {0xff} |
167 {0xff} |
167 }; |
168 }; |
168 #endif |
|
169 |
|
170 /*****************************************************************************/ |
|
171 |
|
172 #if SDO_ACCESS |
|
173 static ec_sdo_request_t *sdo; |
|
174 #endif |
|
175 |
169 |
176 /*****************************************************************************/ |
170 /*****************************************************************************/ |
177 |
171 |
178 void check_domain1_state(void) |
172 void check_domain1_state(void) |
179 { |
173 { |
180 ec_domain_state_t ds; |
174 ec_domain_state_t ds; |
181 |
175 |
182 ecrt_domain_state(domain1, &ds); |
176 ecrt_domain_state(domain1, &ds); |
183 |
177 |
184 if (ds.working_counter != domain1_state.working_counter) |
178 if (ds.working_counter != domain1_state.working_counter) { |
185 printf("Domain1: WC %u.\n", ds.working_counter); |
179 printf("Domain1: WC %u.\n", ds.working_counter); |
186 if (ds.wc_state != domain1_state.wc_state) |
180 } |
|
181 if (ds.wc_state != domain1_state.wc_state) { |
187 printf("Domain1: State %u.\n", ds.wc_state); |
182 printf("Domain1: State %u.\n", ds.wc_state); |
|
183 } |
188 |
184 |
189 domain1_state = ds; |
185 domain1_state = ds; |
190 } |
186 } |
191 |
187 |
192 /*****************************************************************************/ |
188 /*****************************************************************************/ |
195 { |
191 { |
196 ec_master_state_t ms; |
192 ec_master_state_t ms; |
197 |
193 |
198 ecrt_master_state(master, &ms); |
194 ecrt_master_state(master, &ms); |
199 |
195 |
200 if (ms.slaves_responding != master_state.slaves_responding) |
196 if (ms.slaves_responding != master_state.slaves_responding) { |
201 printf("%u slave(s).\n", ms.slaves_responding); |
197 printf("%u slave(s).\n", ms.slaves_responding); |
202 if (ms.al_states != master_state.al_states) |
198 } |
|
199 if (ms.al_states != master_state.al_states) { |
203 printf("AL states: 0x%02X.\n", ms.al_states); |
200 printf("AL states: 0x%02X.\n", ms.al_states); |
204 if (ms.link_up != master_state.link_up) |
201 } |
|
202 if (ms.link_up != master_state.link_up) { |
205 printf("Link is %s.\n", ms.link_up ? "up" : "down"); |
203 printf("Link is %s.\n", ms.link_up ? "up" : "down"); |
|
204 } |
206 |
205 |
207 master_state = ms; |
206 master_state = ms; |
208 } |
207 } |
209 |
208 |
210 /*****************************************************************************/ |
209 /*****************************************************************************/ |
213 { |
212 { |
214 ec_slave_config_state_t s; |
213 ec_slave_config_state_t s; |
215 |
214 |
216 ecrt_slave_config_state(sc_ana_in, &s); |
215 ecrt_slave_config_state(sc_ana_in, &s); |
217 |
216 |
218 if (s.al_state != sc_ana_in_state.al_state) |
217 if (s.al_state != sc_ana_in_state.al_state) { |
219 printf("AnaIn: State 0x%02X.\n", s.al_state); |
218 printf("AnaIn: State 0x%02X.\n", s.al_state); |
220 if (s.online != sc_ana_in_state.online) |
219 } |
|
220 if (s.online != sc_ana_in_state.online) { |
221 printf("AnaIn: %s.\n", s.online ? "online" : "offline"); |
221 printf("AnaIn: %s.\n", s.online ? "online" : "offline"); |
222 if (s.operational != sc_ana_in_state.operational) |
222 } |
223 printf("AnaIn: %soperational.\n", |
223 if (s.operational != sc_ana_in_state.operational) { |
224 s.operational ? "" : "Not "); |
224 printf("AnaIn: %soperational.\n", s.operational ? "" : "Not "); |
|
225 } |
225 |
226 |
226 sc_ana_in_state = s; |
227 sc_ana_in_state = s; |
227 } |
228 } |
228 |
229 |
229 /*****************************************************************************/ |
230 /*****************************************************************************/ |
230 |
|
231 #if SDO_ACCESS |
|
232 void read_sdo(void) |
|
233 { |
|
234 switch (ecrt_sdo_request_state(sdo)) { |
|
235 case EC_REQUEST_UNUSED: // request was not used yet |
|
236 ecrt_sdo_request_read(sdo); // trigger first read |
|
237 break; |
|
238 case EC_REQUEST_BUSY: |
|
239 fprintf(stderr, "Still busy...\n"); |
|
240 break; |
|
241 case EC_REQUEST_SUCCESS: |
|
242 fprintf(stderr, "SDO value: 0x%04X\n", |
|
243 EC_READ_U16(ecrt_sdo_request_data(sdo))); |
|
244 ecrt_sdo_request_read(sdo); // trigger next read |
|
245 break; |
|
246 case EC_REQUEST_ERROR: |
|
247 fprintf(stderr, "Failed to read SDO!\n"); |
|
248 ecrt_sdo_request_read(sdo); // retry reading |
|
249 break; |
|
250 } |
|
251 } |
|
252 #endif |
|
253 |
|
254 /****************************************************************************/ |
|
255 |
231 |
256 void cyclic_task() |
232 void cyclic_task() |
257 { |
233 { |
258 // receive process data |
234 // receive process data |
259 ecrt_master_receive(master); |
235 ecrt_master_receive(master); |
260 ecrt_domain_process(domain1); |
236 ecrt_domain_process(domain1); |
261 |
237 |
262 // check process data state (optional) |
238 // check process data state |
263 check_domain1_state(); |
239 check_domain1_state(); |
264 |
240 |
265 if (counter) { |
241 if (counter) { |
266 counter--; |
242 counter--; |
267 } else { // do this at 1 Hz |
243 } else { // do this at 1 Hz |
300 ecrt_master_send(master); |
270 ecrt_master_send(master); |
301 } |
271 } |
302 |
272 |
303 /****************************************************************************/ |
273 /****************************************************************************/ |
304 |
274 |
305 void signal_handler(int signum) { |
275 void stack_prefault(void) |
306 switch (signum) { |
276 { |
307 case SIGALRM: |
277 unsigned char dummy[MAX_SAFE_STACK]; |
308 sig_alarms++; |
278 |
309 break; |
279 memset(dummy, 0, MAX_SAFE_STACK); |
310 } |
|
311 } |
280 } |
312 |
281 |
313 /****************************************************************************/ |
282 /****************************************************************************/ |
314 |
283 |
315 int main(int argc, char **argv) |
284 int main(int argc, char **argv) |
316 { |
285 { |
317 ec_slave_config_t *sc; |
286 ec_slave_config_t *sc; |
318 struct sigaction sa; |
287 struct timespec wakeup_time; |
319 struct itimerval tv; |
288 int ret = 0; |
320 |
289 |
321 master = ecrt_request_master(0); |
290 master = ecrt_request_master(0); |
322 if (!master) |
291 if (!master) { |
323 return -1; |
292 return -1; |
|
293 } |
324 |
294 |
325 domain1 = ecrt_master_create_domain(master); |
295 domain1 = ecrt_master_create_domain(master); |
326 if (!domain1) |
296 if (!domain1) { |
327 return -1; |
297 return -1; |
|
298 } |
328 |
299 |
329 if (!(sc_ana_in = ecrt_master_slave_config( |
300 if (!(sc_ana_in = ecrt_master_slave_config( |
330 master, AnaInSlavePos, Beckhoff_EL3102))) { |
301 master, AnaInSlavePos, Beckhoff_EL3102))) { |
331 fprintf(stderr, "Failed to get slave configuration.\n"); |
302 fprintf(stderr, "Failed to get slave configuration.\n"); |
332 return -1; |
303 return -1; |
333 } |
304 } |
334 |
305 |
335 #if SDO_ACCESS |
|
336 fprintf(stderr, "Creating SDO requests...\n"); |
|
337 if (!(sdo = ecrt_slave_config_create_sdo_request(sc_ana_in, 0x3102, 2, 2))) { |
|
338 fprintf(stderr, "Failed to create SDO request.\n"); |
|
339 return -1; |
|
340 } |
|
341 ecrt_sdo_request_timeout(sdo, 500); // ms |
|
342 #endif |
|
343 |
|
344 #if CONFIGURE_PDOS |
|
345 printf("Configuring PDOs...\n"); |
306 printf("Configuring PDOs...\n"); |
346 if (ecrt_slave_config_pdos(sc_ana_in, EC_END, el3102_syncs)) { |
307 if (ecrt_slave_config_pdos(sc_ana_in, EC_END, el3102_syncs)) { |
347 fprintf(stderr, "Failed to configure PDOs.\n"); |
308 fprintf(stderr, "Failed to configure PDOs.\n"); |
348 return -1; |
309 return -1; |
349 } |
310 } |
367 |
328 |
368 if (ecrt_slave_config_pdos(sc, EC_END, el2004_syncs)) { |
329 if (ecrt_slave_config_pdos(sc, EC_END, el2004_syncs)) { |
369 fprintf(stderr, "Failed to configure PDOs.\n"); |
330 fprintf(stderr, "Failed to configure PDOs.\n"); |
370 return -1; |
331 return -1; |
371 } |
332 } |
372 #endif |
|
373 |
333 |
374 // Create configuration for bus coupler |
334 // Create configuration for bus coupler |
375 sc = ecrt_master_slave_config(master, BusCouplerPos, Beckhoff_EK1100); |
335 sc = ecrt_master_slave_config(master, BusCouplerPos, Beckhoff_EK1100); |
376 if (!sc) |
336 if (!sc) { |
377 return -1; |
337 return -1; |
|
338 } |
378 |
339 |
379 if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { |
340 if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { |
380 fprintf(stderr, "PDO entry registration failed!\n"); |
341 fprintf(stderr, "PDO entry registration failed!\n"); |
381 return -1; |
342 return -1; |
382 } |
343 } |
383 |
344 |
384 printf("Activating master...\n"); |
345 printf("Activating master...\n"); |
385 if (ecrt_master_activate(master)) |
346 if (ecrt_master_activate(master)) { |
386 return -1; |
347 return -1; |
|
348 } |
387 |
349 |
388 if (!(domain1_pd = ecrt_domain_data(domain1))) { |
350 if (!(domain1_pd = ecrt_domain_data(domain1))) { |
389 return -1; |
351 return -1; |
390 } |
352 } |
391 |
353 |
392 #if PRIORITY |
354 /* Set priority */ |
|
355 |
393 pid_t pid = getpid(); |
356 pid_t pid = getpid(); |
394 if (setpriority(PRIO_PROCESS, pid, -19)) |
357 if (setpriority(PRIO_PROCESS, pid, -19)) { |
395 fprintf(stderr, "Warning: Failed to set priority: %s\n", |
358 fprintf(stderr, "Warning: Failed to set priority: %s\n", |
396 strerror(errno)); |
359 strerror(errno)); |
397 #endif |
360 } |
398 |
361 |
399 sa.sa_handler = signal_handler; |
362 /* Lock memory */ |
400 sigemptyset(&sa.sa_mask); |
363 |
401 sa.sa_flags = 0; |
364 if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) { |
402 if (sigaction(SIGALRM, &sa, 0)) { |
365 fprintf(stderr, "Warning: Failed to lock memory: %s\n", |
403 fprintf(stderr, "Failed to install signal handler!\n"); |
366 strerror(errno)); |
404 return -1; |
367 } |
405 } |
368 |
406 |
369 stack_prefault(); |
407 printf("Starting timer...\n"); |
370 |
408 tv.it_interval.tv_sec = 0; |
371 printf("Starting RT task with dt=%u ns.\n", PERIOD_NS); |
409 tv.it_interval.tv_usec = 1000000 / FREQUENCY; |
372 |
410 tv.it_value.tv_sec = 0; |
373 clock_gettime(CLOCK_MONOTONIC, &wakeup_time); |
411 tv.it_value.tv_usec = 1000; |
374 wakeup_time.tv_sec += 1; /* start in future */ |
412 if (setitimer(ITIMER_REAL, &tv, NULL)) { |
375 wakeup_time.tv_nsec = 0; |
413 fprintf(stderr, "Failed to start timer: %s\n", strerror(errno)); |
376 |
414 return 1; |
|
415 } |
|
416 |
|
417 printf("Started.\n"); |
|
418 while (1) { |
377 while (1) { |
419 pause(); |
378 ret = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, |
420 |
379 &wakeup_time, NULL); |
421 #if 0 |
380 if (ret) { |
422 struct timeval t; |
381 fprintf(stderr, "clock_nanosleep(): %s\n", strerror(ret)); |
423 gettimeofday(&t, NULL); |
382 break; |
424 printf("%u.%06u\n", t.tv_sec, t.tv_usec); |
|
425 #endif |
|
426 |
|
427 while (sig_alarms != user_alarms) { |
|
428 cyclic_task(); |
|
429 user_alarms++; |
|
430 } |
383 } |
431 } |
384 |
432 |
385 cyclic_task(); |
433 return 0; |
386 |
434 } |
387 wakeup_time.tv_nsec += PERIOD_NS; |
435 |
388 while (wakeup_time.tv_nsec >= NSEC_PER_SEC) { |
436 /****************************************************************************/ |
389 wakeup_time.tv_nsec -= NSEC_PER_SEC; |
|
390 wakeup_time.tv_sec++; |
|
391 } |
|
392 } |
|
393 |
|
394 return ret; |
|
395 } |
|
396 |
|
397 /****************************************************************************/ |