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