69 |
69 |
70 #define BusCoupler01_Pos 0, 0 |
70 #define BusCoupler01_Pos 0, 0 |
71 #define DigOutSlave01_Pos 0, 1 |
71 #define DigOutSlave01_Pos 0, 1 |
72 |
72 |
73 #define Beckhoff_EK1100 0x00000002, 0x044c2c52 |
73 #define Beckhoff_EK1100 0x00000002, 0x044c2c52 |
74 #define Beckhoff_EL2004 0x00000002, 0x07d43052 |
74 #define Beckhoff_EL2088 0x00000002, 0x08283052 |
75 |
75 |
76 // offsets for PDO entries |
76 // offsets for PDO entries |
77 static unsigned int off_dig_out0 = 0; |
77 static unsigned int off_dig_out0 = 0; |
78 |
78 |
79 // process data |
79 // process data |
80 |
80 |
81 const static ec_pdo_entry_reg_t domain1_regs[] = { |
81 const static ec_pdo_entry_reg_t domain1_regs[] = { |
82 {DigOutSlave01_Pos, Beckhoff_EL2004, 0x7000, 0x01, &off_dig_out0, NULL}, |
82 {DigOutSlave01_Pos, Beckhoff_EL2088, 0x7000, 0x01, &off_dig_out0, NULL}, |
83 {} |
83 {} |
84 }; |
84 }; |
85 |
85 |
86 /****************************************************************************/ |
86 /****************************************************************************/ |
87 |
87 |
159 void *my_thread(void *arg) |
159 void *my_thread(void *arg) |
160 { |
160 { |
161 struct timespec next_period; |
161 struct timespec next_period; |
162 int cycle_counter = 0; |
162 int cycle_counter = 0; |
163 unsigned int blink = 0; |
163 unsigned int blink = 0; |
164 |
164 struct sched_param param = { .sched_priority = 80 }; |
165 clock_gettime(CLOCK_REALTIME, &next_period); |
165 |
|
166 pthread_setschedparam(pthread_self(), SCHED_FIFO, ¶m); |
|
167 |
|
168 clock_gettime(CLOCK_MONOTONIC, &next_period); |
166 |
169 |
167 while (run) { |
170 while (run) { |
168 next_period.tv_nsec += cycle_us * 1000; |
171 next_period.tv_nsec += cycle_us * 1000; |
169 while (next_period.tv_nsec >= NSEC_PER_SEC) { |
172 while (next_period.tv_nsec >= NSEC_PER_SEC) { |
170 next_period.tv_nsec -= NSEC_PER_SEC; |
173 next_period.tv_nsec -= NSEC_PER_SEC; |
171 next_period.tv_sec++; |
174 next_period.tv_sec++; |
172 } |
175 } |
173 |
176 |
174 clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &next_period, NULL); |
177 clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_period, NULL); |
175 |
178 |
176 cycle_counter++; |
179 cycle_counter++; |
177 |
180 |
178 // receive EtherCAT |
181 // receive EtherCAT |
179 ecrt_master_receive(master); |
182 ecrt_master_receive(master); |
265 |
268 |
266 if (!(domain1_pd = ecrt_domain_data(domain1))) { |
269 if (!(domain1_pd = ecrt_domain_data(domain1))) { |
267 fprintf(stderr, "Failed to get domain data pointer.\n"); |
270 fprintf(stderr, "Failed to get domain data pointer.\n"); |
268 return -1; |
271 return -1; |
269 } |
272 } |
270 |
|
271 /* Create cyclic RT-thread */ |
|
272 struct sched_param param = { .sched_priority = 82 }; |
|
273 |
273 |
274 pthread_attr_t thattr; |
274 pthread_attr_t thattr; |
275 pthread_attr_init(&thattr); |
275 pthread_attr_init(&thattr); |
276 pthread_attr_setdetachstate(&thattr, PTHREAD_CREATE_JOINABLE); |
276 pthread_attr_setdetachstate(&thattr, PTHREAD_CREATE_JOINABLE); |
277 pthread_attr_setinheritsched(&thattr, PTHREAD_EXPLICIT_SCHED); |
|
278 pthread_attr_setschedpolicy(&thattr, SCHED_FIFO); |
|
279 pthread_setschedparam(cyclic_thread, SCHED_FIFO, ¶m); |
|
280 |
277 |
281 ret = pthread_create(&cyclic_thread, &thattr, &my_thread, NULL); |
278 ret = pthread_create(&cyclic_thread, &thattr, &my_thread, NULL); |
282 if (ret) { |
279 if (ret) { |
283 fprintf(stderr, "%s: pthread_create cyclic task failed\n", |
280 fprintf(stderr, "%s: pthread_create cyclic task failed\n", |
284 strerror(-ret)); |
281 strerror(-ret)); |