45 |
45 |
46 /*****************************************************************************/ |
46 /*****************************************************************************/ |
47 |
47 |
48 // Module parameters |
48 // Module parameters |
49 |
49 |
50 #define FREQUENCY 2000 // task frequency in Hz |
50 #define FREQUENCY 1000 // task frequency in Hz |
51 #define INHIBIT_TIME 20 |
51 #define INHIBIT_TIME 20 |
52 |
52 |
53 #define TIMERTICKS (1000000000 / FREQUENCY) |
53 #define TIMERTICKS (1000000000 / FREQUENCY) |
54 |
54 |
55 // Optional features (comment to disable) |
55 // Optional features (comment to disable) |
92 const static ec_pdo_entry_reg_t domain1_regs[] = { |
92 const static ec_pdo_entry_reg_t domain1_regs[] = { |
93 {AnaInSlavePos, Beckhoff_EL3162, 0x3101, 2, &off_ana_in}, |
93 {AnaInSlavePos, Beckhoff_EL3162, 0x3101, 2, &off_ana_in}, |
94 {DigOutSlavePos, Beckhoff_EL2004, 0x3001, 1, &off_dig_out}, |
94 {DigOutSlavePos, Beckhoff_EL2004, 0x3001, 1, &off_dig_out}, |
95 {} |
95 {} |
96 }; |
96 }; |
97 |
|
98 static unsigned int counter = 0; |
|
99 static unsigned int blink = 0; |
|
100 |
97 |
101 /*****************************************************************************/ |
98 /*****************************************************************************/ |
102 |
99 |
103 #ifdef CONFIGURE_PDOS |
100 #ifdef CONFIGURE_PDOS |
104 static ec_pdo_entry_info_t el3162_channel1[] = { |
101 static ec_pdo_entry_info_t el3162_channel1[] = { |
202 sc_ana_in_state = s; |
199 sc_ana_in_state = s; |
203 } |
200 } |
204 |
201 |
205 /*****************************************************************************/ |
202 /*****************************************************************************/ |
206 |
203 |
|
204 #define US(x) ((unsigned int) (x) * 1000 / cpu_khz) |
|
205 |
207 void run(long data) |
206 void run(long data) |
208 { |
207 { |
|
208 cycles_t c0, c1, c2, c3, c4; |
|
209 unsigned int c = 10000; |
|
210 |
209 while (1) { |
211 while (1) { |
210 t_last_cycle = get_cycles(); |
212 t_last_cycle = get_cycles(); |
211 |
213 |
212 // receive process data |
214 c0 = get_cycles(); |
213 rt_sem_wait(&master_sem); |
|
214 ecrt_master_receive(master); |
215 ecrt_master_receive(master); |
|
216 c1 = get_cycles(); |
215 ecrt_domain_process(domain1); |
217 ecrt_domain_process(domain1); |
216 rt_sem_signal(&master_sem); |
218 c2 = get_cycles(); |
217 |
219 ecrt_domain_queue(domain1); |
218 // check process data state (optional) |
220 c3 = get_cycles(); |
219 check_domain1_state(); |
221 ecrt_master_send(master); |
220 |
222 c4 = get_cycles(); |
221 if (counter) { |
223 |
222 counter--; |
224 if (c) { |
223 } else { // do this at 1 Hz |
225 printk("TTTT4 %6u %4u %4u %4u %4u\n", |
224 counter = FREQUENCY; |
226 c, |
225 |
227 US(c1 - c0), |
226 // calculate new process data |
228 US(c2 - c1), |
227 blink = !blink; |
229 US(c3 - c2), |
228 |
230 US(c4 - c3)); |
229 // check for master state (optional) |
231 c--; |
230 check_master_state(); |
|
231 |
|
232 // check for islave configuration state(s) (optional) |
|
233 check_slave_config_states(); |
|
234 } |
232 } |
235 |
|
236 // write process data |
|
237 EC_WRITE_U8(domain1_pd + off_dig_out, blink ? 0x06 : 0x09); |
|
238 |
|
239 rt_sem_wait(&master_sem); |
|
240 ecrt_domain_queue(domain1); |
|
241 ecrt_master_send(master); |
|
242 rt_sem_signal(&master_sem); |
|
243 |
233 |
244 rt_task_wait_period(); |
234 rt_task_wait_period(); |
245 } |
235 } |
246 } |
236 } |
247 |
237 |