|
1 /****************************************************************************** |
|
2 * |
|
3 * $Id$ |
|
4 * |
|
5 * Copyright (C) 2011 IgH Andreas Stewering-Bone |
|
6 * 2012 Florian Pose <fp@igh-essen.com> |
|
7 * |
|
8 * This file is part of the IgH EtherCAT master |
|
9 * |
|
10 * The IgH EtherCAT Master is free software; you can redistribute it and/or |
|
11 * modify it under the terms of the GNU General Public License version 2, as |
|
12 * published by the Free Software Foundation. |
|
13 * |
|
14 * The IgH EtherCAT master is distributed in the hope that it will be useful, |
|
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General |
|
17 * Public License for more details. |
|
18 * |
|
19 * You should have received a copy of the GNU General Public License along |
|
20 * with the IgH EtherCAT master. If not, see <http://www.gnu.org/licenses/>. |
|
21 * |
|
22 * --- |
|
23 * |
|
24 * The license mentioned above concerns the source code only. Using the |
|
25 * EtherCAT technology and brand is only permitted in compliance with the |
|
26 * industrial property and similar rights of Beckhoff Automation GmbH. |
|
27 * |
|
28 *****************************************************************************/ |
|
29 |
|
30 #include <sched.h> |
|
31 #include <stdio.h> |
|
32 #include <stdlib.h> |
|
33 #include <fcntl.h> |
|
34 #include <signal.h> |
|
35 |
|
36 #include <rtai_lxrt.h> |
|
37 #include <rtdm/rtdm.h> |
|
38 |
|
39 #include "ecrt.h" |
|
40 |
|
41 #define rt_printf(X, Y) |
|
42 |
|
43 #define NSEC_PER_SEC 1000000000 |
|
44 |
|
45 RT_TASK *task; |
|
46 |
|
47 static unsigned int cycle_ns = 1000000; /* 1 ms */ |
|
48 |
|
49 static int run = 1; |
|
50 |
|
51 /****************************************************************************/ |
|
52 |
|
53 // EtherCAT |
|
54 static ec_master_t *master = NULL; |
|
55 static ec_master_state_t master_state = {}; |
|
56 |
|
57 static ec_domain_t *domain1 = NULL; |
|
58 static ec_domain_state_t domain1_state = {}; |
|
59 |
|
60 static uint8_t *domain1_pd = NULL; |
|
61 |
|
62 static ec_slave_config_t *sc_dig_out_01 = NULL; |
|
63 |
|
64 /****************************************************************************/ |
|
65 |
|
66 // EtherCAT distributed clock variables |
|
67 |
|
68 #define DC_FILTER_CNT 1024 |
|
69 #define SYNC_MASTER_TO_REF 1 |
|
70 |
|
71 static uint64_t dc_start_time_ns = 0LL; |
|
72 static uint64_t dc_time_ns = 0; |
|
73 #if SYNC_MASTER_TO_REF |
|
74 static uint8_t dc_started = 0; |
|
75 static int32_t dc_diff_ns = 0; |
|
76 static int32_t prev_dc_diff_ns = 0; |
|
77 static int64_t dc_diff_total_ns = 0LL; |
|
78 static int64_t dc_delta_total_ns = 0LL; |
|
79 static int dc_filter_idx = 0; |
|
80 static int64_t dc_adjust_ns; |
|
81 #endif |
|
82 static int64_t system_time_base = 0LL; |
|
83 static uint64_t wakeup_time = 0LL; |
|
84 static uint64_t overruns = 0LL; |
|
85 |
|
86 /****************************************************************************/ |
|
87 |
|
88 // process data |
|
89 |
|
90 #define BusCoupler01_Pos 0, 0 |
|
91 #define DigOutSlave01_Pos 0, 1 |
|
92 |
|
93 #define Beckhoff_EK1100 0x00000002, 0x044c2c52 |
|
94 #define Beckhoff_EL2004 0x00000002, 0x07d43052 |
|
95 |
|
96 // offsets for PDO entries |
|
97 static unsigned int off_dig_out0 = 0; |
|
98 |
|
99 // process data |
|
100 |
|
101 const static ec_pdo_entry_reg_t domain1_regs[] = { |
|
102 {DigOutSlave01_Pos, Beckhoff_EL2004, 0x7000, 0x01, &off_dig_out0, NULL}, |
|
103 {} |
|
104 }; |
|
105 |
|
106 /****************************************************************************/ |
|
107 |
|
108 /* Slave 1, "EL2004" |
|
109 * Vendor ID: 0x00000002 |
|
110 * Product code: 0x07d43052 |
|
111 * Revision number: 0x00100000 |
|
112 */ |
|
113 |
|
114 ec_pdo_entry_info_t slave_1_pdo_entries[] = { |
|
115 {0x7000, 0x01, 1}, /* Output */ |
|
116 {0x7010, 0x01, 1}, /* Output */ |
|
117 {0x7020, 0x01, 1}, /* Output */ |
|
118 {0x7030, 0x01, 1}, /* Output */ |
|
119 }; |
|
120 |
|
121 ec_pdo_info_t slave_1_pdos[] = { |
|
122 {0x1600, 1, slave_1_pdo_entries + 0}, /* Channel 1 */ |
|
123 {0x1601, 1, slave_1_pdo_entries + 1}, /* Channel 2 */ |
|
124 {0x1602, 1, slave_1_pdo_entries + 2}, /* Channel 3 */ |
|
125 {0x1603, 1, slave_1_pdo_entries + 3}, /* Channel 4 */ |
|
126 }; |
|
127 |
|
128 ec_sync_info_t slave_1_syncs[] = { |
|
129 {0, EC_DIR_OUTPUT, 4, slave_1_pdos + 0, EC_WD_ENABLE}, |
|
130 {0xff} |
|
131 }; |
|
132 |
|
133 /***************************************************************************** |
|
134 * Realtime task |
|
135 ****************************************************************************/ |
|
136 |
|
137 /** Get the time in ns for the current cpu, adjusted by system_time_base. |
|
138 * |
|
139 * \attention Rather than calling rt_get_time_ns() directly, all application |
|
140 * time calls should use this method instead. |
|
141 * |
|
142 * \ret The time in ns. |
|
143 */ |
|
144 uint64_t system_time_ns(void) |
|
145 { |
|
146 RTIME time = rt_get_time_ns(); |
|
147 |
|
148 if (system_time_base > time) { |
|
149 rt_printk("%s() error: system_time_base greater than" |
|
150 " system time (system_time_base: %lld, time: %llu\n", |
|
151 __func__, system_time_base, time); |
|
152 return time; |
|
153 } |
|
154 else { |
|
155 return time - system_time_base; |
|
156 } |
|
157 } |
|
158 |
|
159 /****************************************************************************/ |
|
160 |
|
161 /** Convert system time to RTAI time in counts (via the system_time_base). |
|
162 */ |
|
163 RTIME system2count( |
|
164 uint64_t time |
|
165 ) |
|
166 { |
|
167 RTIME ret; |
|
168 |
|
169 if ((system_time_base < 0) && |
|
170 ((uint64_t) (-system_time_base) > time)) { |
|
171 rt_printk("%s() error: system_time_base less than" |
|
172 " system time (system_time_base: %lld, time: %llu\n", |
|
173 __func__, system_time_base, time); |
|
174 ret = time; |
|
175 } |
|
176 else { |
|
177 ret = time + system_time_base; |
|
178 } |
|
179 |
|
180 return nano2count(ret); |
|
181 } |
|
182 |
|
183 /*****************************************************************************/ |
|
184 |
|
185 /** Synchronise the distributed clocks |
|
186 */ |
|
187 void sync_distributed_clocks(void) |
|
188 { |
|
189 #if SYNC_MASTER_TO_REF |
|
190 uint32_t ref_time = 0; |
|
191 uint64_t prev_app_time = dc_time_ns; |
|
192 #endif |
|
193 |
|
194 dc_time_ns = system_time_ns(); |
|
195 |
|
196 // set master time in nano-seconds |
|
197 ecrt_master_application_time(master, dc_time_ns); |
|
198 |
|
199 #if SYNC_MASTER_TO_REF |
|
200 // get reference clock time to synchronize master cycle |
|
201 ecrt_master_reference_clock_time(master, &ref_time); |
|
202 dc_diff_ns = (uint32_t) prev_app_time - ref_time; |
|
203 #else |
|
204 // sync reference clock to master |
|
205 ecrt_master_sync_reference_clock(master); |
|
206 #endif |
|
207 |
|
208 // call to sync slaves to ref slave |
|
209 ecrt_master_sync_slave_clocks(master); |
|
210 } |
|
211 |
|
212 /*****************************************************************************/ |
|
213 |
|
214 /** Return the sign of a number |
|
215 * |
|
216 * ie -1 for -ve value, 0 for 0, +1 for +ve value |
|
217 * |
|
218 * \retval the sign of the value |
|
219 */ |
|
220 #define sign(val) \ |
|
221 ({ typeof (val) _val = (val); \ |
|
222 ((_val > 0) - (_val < 0)); }) |
|
223 |
|
224 /*****************************************************************************/ |
|
225 |
|
226 /** Update the master time based on ref slaves time diff |
|
227 * |
|
228 * called after the ethercat frame is sent to avoid time jitter in |
|
229 * sync_distributed_clocks() |
|
230 */ |
|
231 void update_master_clock(void) |
|
232 { |
|
233 #if SYNC_MASTER_TO_REF |
|
234 // calc drift (via un-normalised time diff) |
|
235 int32_t delta = dc_diff_ns - prev_dc_diff_ns; |
|
236 prev_dc_diff_ns = dc_diff_ns; |
|
237 |
|
238 // normalise the time diff |
|
239 dc_diff_ns = |
|
240 ((dc_diff_ns + (cycle_ns / 2)) % cycle_ns) - (cycle_ns / 2); |
|
241 |
|
242 // only update if primary master |
|
243 if (dc_started) { |
|
244 |
|
245 // add to totals |
|
246 dc_diff_total_ns += dc_diff_ns; |
|
247 dc_delta_total_ns += delta; |
|
248 dc_filter_idx++; |
|
249 |
|
250 if (dc_filter_idx >= DC_FILTER_CNT) { |
|
251 // add rounded delta average |
|
252 dc_adjust_ns += |
|
253 ((dc_delta_total_ns + (DC_FILTER_CNT / 2)) / DC_FILTER_CNT); |
|
254 |
|
255 // and add adjustment for general diff (to pull in drift) |
|
256 dc_adjust_ns += sign(dc_diff_total_ns / DC_FILTER_CNT); |
|
257 |
|
258 // limit crazy numbers (0.1% of std cycle time) |
|
259 if (dc_adjust_ns < -1000) { |
|
260 dc_adjust_ns = -1000; |
|
261 } |
|
262 if (dc_adjust_ns > 1000) { |
|
263 dc_adjust_ns = 1000; |
|
264 } |
|
265 |
|
266 // reset |
|
267 dc_diff_total_ns = 0LL; |
|
268 dc_delta_total_ns = 0LL; |
|
269 dc_filter_idx = 0; |
|
270 } |
|
271 |
|
272 // add cycles adjustment to time base (including a spot adjustment) |
|
273 system_time_base += dc_adjust_ns + sign(dc_diff_ns); |
|
274 } |
|
275 else { |
|
276 dc_started = (dc_diff_ns != 0); |
|
277 |
|
278 if (dc_started) { |
|
279 // output first diff |
|
280 rt_printk("First master diff: %d.\n", dc_diff_ns); |
|
281 |
|
282 // record the time of this initial cycle |
|
283 dc_start_time_ns = dc_time_ns; |
|
284 } |
|
285 } |
|
286 #endif |
|
287 } |
|
288 |
|
289 /****************************************************************************/ |
|
290 |
|
291 void rt_check_domain_state(void) |
|
292 { |
|
293 ec_domain_state_t ds = {}; |
|
294 |
|
295 ecrt_domain_state(domain1, &ds); |
|
296 |
|
297 if (ds.working_counter != domain1_state.working_counter) { |
|
298 rt_printf("Domain1: WC %u.\n", ds.working_counter); |
|
299 } |
|
300 |
|
301 if (ds.wc_state != domain1_state.wc_state) { |
|
302 rt_printf("Domain1: State %u.\n", ds.wc_state); |
|
303 } |
|
304 |
|
305 domain1_state = ds; |
|
306 } |
|
307 |
|
308 /****************************************************************************/ |
|
309 |
|
310 void rt_check_master_state(void) |
|
311 { |
|
312 ec_master_state_t ms; |
|
313 |
|
314 ecrt_master_state(master, &ms); |
|
315 |
|
316 if (ms.slaves_responding != master_state.slaves_responding) { |
|
317 rt_printf("%u slave(s).\n", ms.slaves_responding); |
|
318 } |
|
319 |
|
320 if (ms.al_states != master_state.al_states) { |
|
321 rt_printf("AL states: 0x%02X.\n", ms.al_states); |
|
322 } |
|
323 |
|
324 if (ms.link_up != master_state.link_up) { |
|
325 rt_printf("Link is %s.\n", ms.link_up ? "up" : "down"); |
|
326 } |
|
327 |
|
328 master_state = ms; |
|
329 } |
|
330 |
|
331 /****************************************************************************/ |
|
332 |
|
333 /** Wait for the next period |
|
334 */ |
|
335 void wait_period(void) |
|
336 { |
|
337 while (1) |
|
338 { |
|
339 RTIME wakeup_count = system2count(wakeup_time); |
|
340 RTIME current_count = rt_get_time(); |
|
341 |
|
342 if ((wakeup_count < current_count) |
|
343 || (wakeup_count > current_count + (50 * cycle_ns))) { |
|
344 rt_printk("%s(): unexpected wake time!\n", __func__); |
|
345 } |
|
346 |
|
347 switch (rt_sleep_until(wakeup_count)) { |
|
348 case RTE_UNBLKD: |
|
349 rt_printk("rt_sleep_until(): RTE_UNBLKD\n"); |
|
350 continue; |
|
351 |
|
352 case RTE_TMROVRN: |
|
353 rt_printk("rt_sleep_until(): RTE_TMROVRN\n"); |
|
354 overruns++; |
|
355 |
|
356 if (overruns % 100 == 0) { |
|
357 // in case wake time is broken ensure other processes get |
|
358 // some time slice (and error messages can get displayed) |
|
359 rt_sleep(cycle_ns / 100); |
|
360 } |
|
361 break; |
|
362 |
|
363 default: |
|
364 break; |
|
365 } |
|
366 |
|
367 // done if we got to here |
|
368 break; |
|
369 } |
|
370 |
|
371 // calc next wake time (in sys time) |
|
372 wakeup_time += cycle_ns; |
|
373 } |
|
374 |
|
375 /****************************************************************************/ |
|
376 |
|
377 void my_cyclic(void) |
|
378 { |
|
379 int cycle_counter = 0; |
|
380 unsigned int blink = 0; |
|
381 |
|
382 // oneshot mode to allow adjustable wake time |
|
383 rt_set_oneshot_mode(); |
|
384 |
|
385 // set first wake time in a few cycles |
|
386 wakeup_time = system_time_ns() + 10 * cycle_ns; |
|
387 |
|
388 // start the timer |
|
389 start_rt_timer(nano2count(cycle_ns)); |
|
390 |
|
391 rt_make_hard_real_time(); |
|
392 |
|
393 while (run) { |
|
394 // wait for next period (using adjustable system time) |
|
395 wait_period(); |
|
396 |
|
397 cycle_counter++; |
|
398 |
|
399 if (!run) { |
|
400 break; |
|
401 } |
|
402 |
|
403 // receive EtherCAT |
|
404 ecrt_master_receive(master); |
|
405 ecrt_domain_process(domain1); |
|
406 |
|
407 rt_check_domain_state(); |
|
408 |
|
409 if (!(cycle_counter % 1000)) { |
|
410 rt_check_master_state(); |
|
411 } |
|
412 |
|
413 if (!(cycle_counter % 200)) { |
|
414 blink = !blink; |
|
415 } |
|
416 |
|
417 EC_WRITE_U8(domain1_pd + off_dig_out0, blink ? 0x00 : 0x0F); |
|
418 |
|
419 // queue process data |
|
420 ecrt_domain_queue(domain1); |
|
421 |
|
422 // sync distributed clock just before master_send to set |
|
423 // most accurate master clock time |
|
424 sync_distributed_clocks(); |
|
425 |
|
426 // send EtherCAT data |
|
427 ecrt_master_send(master); |
|
428 |
|
429 // update the master clock |
|
430 // Note: called after ecrt_master_send() to reduce time |
|
431 // jitter in the sync_distributed_clocks() call |
|
432 update_master_clock(); |
|
433 } |
|
434 |
|
435 rt_make_soft_real_time(); |
|
436 stop_rt_timer(); |
|
437 } |
|
438 |
|
439 /**************************************************************************** |
|
440 * Signal handler |
|
441 ***************************************************************************/ |
|
442 |
|
443 void signal_handler(int sig) |
|
444 { |
|
445 run = 0; |
|
446 } |
|
447 |
|
448 /**************************************************************************** |
|
449 * Main function |
|
450 ***************************************************************************/ |
|
451 |
|
452 int main(int argc, char *argv[]) |
|
453 { |
|
454 ec_slave_config_t *sc_ek1100; |
|
455 int ret; |
|
456 |
|
457 signal(SIGTERM, signal_handler); |
|
458 signal(SIGINT, signal_handler); |
|
459 |
|
460 mlockall(MCL_CURRENT | MCL_FUTURE); |
|
461 |
|
462 printf("Requesting master...\n"); |
|
463 master = ecrt_request_master(0); |
|
464 if (!master) { |
|
465 return -1; |
|
466 } |
|
467 |
|
468 domain1 = ecrt_master_create_domain(master); |
|
469 if (!domain1) { |
|
470 return -1; |
|
471 } |
|
472 |
|
473 printf("Creating slave configurations...\n"); |
|
474 |
|
475 // Create configuration for bus coupler |
|
476 sc_ek1100 = |
|
477 ecrt_master_slave_config(master, BusCoupler01_Pos, Beckhoff_EK1100); |
|
478 if (!sc_ek1100) { |
|
479 return -1; |
|
480 } |
|
481 |
|
482 sc_dig_out_01 = |
|
483 ecrt_master_slave_config(master, DigOutSlave01_Pos, Beckhoff_EL2004); |
|
484 if (!sc_dig_out_01) { |
|
485 fprintf(stderr, "Failed to get slave configuration.\n"); |
|
486 return -1; |
|
487 } |
|
488 |
|
489 if (ecrt_slave_config_pdos(sc_dig_out_01, EC_END, slave_1_syncs)) { |
|
490 fprintf(stderr, "Failed to configure PDOs.\n"); |
|
491 return -1; |
|
492 } |
|
493 |
|
494 if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { |
|
495 fprintf(stderr, "PDO entry registration failed!\n"); |
|
496 return -1; |
|
497 } |
|
498 |
|
499 /* Set the initial master time and select a slave to use as the DC |
|
500 * reference clock, otherwise pass NULL to auto select the first capable |
|
501 * slave. Note: This can be used whether the master or the ref slave will |
|
502 * be used as the systems master DC clock. |
|
503 */ |
|
504 dc_start_time_ns = system_time_ns(); |
|
505 dc_time_ns = dc_start_time_ns; |
|
506 |
|
507 /* Attention: The initial application time is also used for phase |
|
508 * calculation for the SYNC0/1 interrupts. Please be sure to call it at |
|
509 * the correct phase to the realtime cycle. |
|
510 */ |
|
511 ecrt_master_application_time(master, dc_start_time_ns); |
|
512 |
|
513 ret = ecrt_master_select_reference_clock(master, sc_ek1100); |
|
514 if (ret < 0) { |
|
515 fprintf(stderr, "Failed to select reference clock: %s\n", |
|
516 strerror(-ret)); |
|
517 return ret; |
|
518 } |
|
519 |
|
520 printf("Activating master...\n"); |
|
521 if (ecrt_master_activate(master)) { |
|
522 return -1; |
|
523 } |
|
524 |
|
525 if (!(domain1_pd = ecrt_domain_data(domain1))) { |
|
526 fprintf(stderr, "Failed to get domain data pointer.\n"); |
|
527 return -1; |
|
528 } |
|
529 |
|
530 /* Create cyclic RT-thread */ |
|
531 struct sched_param param; |
|
532 param.sched_priority = sched_get_priority_max(SCHED_FIFO) - 1; |
|
533 if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) { |
|
534 puts("ERROR IN SETTING THE SCHEDULER"); |
|
535 perror("errno"); |
|
536 return -1; |
|
537 } |
|
538 |
|
539 task = rt_task_init(nam2num("ec_rtai_rtdm_example"), |
|
540 0 /* priority */, 0 /* stack size */, 0 /* msg size */); |
|
541 |
|
542 my_cyclic(); |
|
543 |
|
544 rt_task_delete(task); |
|
545 |
|
546 printf("End of Program\n"); |
|
547 ecrt_release_master(master); |
|
548 |
|
549 return 0; |
|
550 } |
|
551 |
|
552 /****************************************************************************/ |